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ABSTRACT 


Title  of  Thesis:  IMPACT  AND  FORCE  CONTROL  OF  FLEXIBLE 
MANIPULATORS 


Name  of  degree  candidate:  Ioanis  Salmatjidis 


Degree  and  Year:  Master  of  Science,  1991 


Thesis  directed  by:  P.  S.  Krishnaprasad 
Professor 

Electrical  Engineering 


We  consider  the  force  control  problem  of  a  one  degree-of-freedom  flexible 
robot  manipulator.  We  approximate  the  distributed  parameter  flexible  struc¬ 
ture  by  a  finite  number  of  rigid  elements  connected  by  means  of  torsional  springs. 
We  assume  that  the  arm  tip  interacts  with  the  environment  under  rigid,  fric¬ 
tionless,  point  contact  conditions.  The  kinematic  (holonomic)  constraints  which 
hold  when  contact  is  established,  are  derived  using  the  geometry  of  the  prob¬ 
lem.  The  free  and  constrained  motion  of  the  arm  are  predicted  numerically 
using  the  Newmark  integration  method.  Numerical  results  are  compared  to  the 


empirical  system  response.  The  conventional  energy  principle  method  for  pre¬ 
dicting  the  maximum  reaction  force  is  demonstrated,  along  with  a  much  more 
efficient  method  based  on  the  evaluation  of  instantaneous  velocity  increments 
just  after  impact.  The  latter  assumes  that  velocities  vary  linearly  between  the 
time  instants  of  initial  contact  and  maximum  force  occurrence.  Finally,  a  hy¬ 
brid  impact-force  real-time  controller  for  diminishing  the  impact  effect  is  imple¬ 
mented  and  various  design  considerations  are  presented. 
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Chapter  1 


Introduction 


Research  work  in  impact  dynamics  as  it  pertains  to  robotics  appears  to  be  scant 
in  compeixison  to  work  on  force  and  trajectory  control  of  robot  manipulators.  In 
the  context  of  trajectory  control,  the  manipulator  moves  in  a  free  space  without 
any  interaction  with  the  environment.  In  force  control  a  manipulator  interacts 
with  the  environment  while  the  controller  attempts  to  maintain  a  force  trajec¬ 
tory.  Such  applications  include  assembly  lines,  polishing  etc.  In  the  transition 
between  the  no-interaction  and  interaction  modes,  impact  occurs.  During  the 
impact  phenomena,  which  last  only  short  periods  of  time,  energy  transfer  takes 
place.  Generally,  high  forces  appear  at  the  point  or  points  of  contact  and  at  the 
joints  of  the  manipulator  as  well,  which  could  be  harmful  to  the  robot  or  the 
workpiece. 

As  far  as  controller  design  is  concerned,  there  are  three  modes  of  operation 
so  that  a  robot  completes  a  task:  motion  in  free  space,  impact  and  constrained 
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motion.  For  different  tasks  the  strategy  for  the  task  to  be  completed  might  be 
different.  Therefore,  the  controller  should  decide  what  kinds  of  operations  are 
needed  for  each  mode  and  when  to  change  from  one  mode  to  another.  In  each 
mode,  different  kinds  of  feedback  information  might  be  used.  For  example,  in  the 
unconstrained  subspace  of  the  state  space,  it  is  the  position  of  the  end-effector  to 
be  specified  and  controlled,  while  in  the  constrained  subspace  it  is  the  torque  or 
force  feedback  that  determine  the  control  actions.  The  controller  design  should 
be  able  to  interpret  this  feedback  information  to  generate  the  actuator  inputs. 
Generally,  switching  from  constrained  force  control  to  unconstrained  position 
control  presents  no  problems.  The  opposite,  however,  has  the  significant  problem 
of  impact  forces.  The  physical  processes  taking  place  during  this  phase  are 
highly  complex.  Previous  research  has  treated  the  impact  phase  as  a  transient 
that  is  dealt  with  by  the  same  controller  that  follows  some  desired  contact  force 
command  and  in  some  way  has  ignored  this  important  transitory  period. 

Matters  get  more  complicated  when  flexibility  of  links  or  joints,  that  creates 
undesirable  vibrational  effects  is  present.  While  the  high  modes  of  the  impact 
phenomena  may  be  absorbed,  allowing  the  controller  more  time  to  update  the 
actuator  input,  extensive  understanding  of  the  dynamics  of  such  flexible  struc¬ 
tures  is  required.  For  example,  the  naive  control  law  which  dictates  to  move 
the  arm  till  contact  is  achieved  and  then  to  apply  the  maximum  torque  in  the 
opposite  direction  may  work  for  rigid  manipulators  but  this  is  not  the  case  for 
flexible  ones,  since  inertia  and  flexibility  contribute  to  negative  acceleration  at 
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the  end-effector. 


This  thesis  considers  the  problem  of  understanding  the  details  of  the  impact 
phenomenon  that  takes  place  at  the  tip  of  a  light-weight,  single  link,  flexible 
robot  arm.  The  final  objective  is  to  design  a  control  system  that  passes  the 
impact  transient  successfully  and  causes  the  manipulator  to  stably  exert  some 
desired  force  on  the  workpiece  at  the  end.  The  single  link  flexible  manipulator 
is  obviously  a  distributed  parameter  system  and  therefore  the  first  task  is  to 
choose  a  suitable  approximate,  finite  dimensional  model.  A  second  order  finite 
element  approximation  (i.e.  the  flexible  link  is  represented  by  two  rigid  bodies 
coupled  by  revolute  joints)  is  used  throughout  this  work.  The  model  is  justified 
after  comparisons  between  the  simulation  results  and  the  experimental  system 
response,  prior  to  the  design  of  the  controller.  Simulations  include  free  beam 
motion  prediction  as  well  as  constrained  beam  motion  prediction. 

The  knowledge  of  the  maximum  impact  force  that  will  be  developed  at  the 
arm  tip  on  an  impending  contact  event  might  be  helpful,  if  not  necessary,  to 
correct  the  position  controller  while  still  in  free  space.  The  conventional  and 
less  accurate  energy  principle  method  is  demonstrated  in  Chapter  4  ,  along  with 
a  much  more  efficient  method  based  on  the  evaluation  of  instantaneous  velocity 
increments  immediately  after  contact. 

The  last  chapter  describes  the  real-time  implementation  of  a  hybrid  impact- 
force  controller.  Previous  schemes  of  impact  control  are  briefly  discussed  and  a 
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proposed  impact-force  control  strategy  is  presented  for  the  available  experimen¬ 
tal  set-up.  The  second  order  finite  element  approximation  is  proven  necessary,  as 
computing  power  limitations  and  insufficient  state  measurements  appear  more 
acute  in  controlling  such  short  transient  events.  It  is  useful  to  note  that  except 
for  the  case  of  the  controller  implementation,  the  work  presented  here  can  be  be 
easily  extended  and  improved  by  a  higher  order  approximation. 
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Chapter  2 

Experimental  setup  and  modeling 

2.1  Hardware  description 

The  experimental  setup  consists  of  a  single-link  robot  arm  with  link  flexibility, 
an  IBM  PC- AT  computer  with  A/D  and  D/A  interface  capabilities  and  three 
sensors.  The  flexible  beam  is  excited  by  a  DC  brushless  torque  motor.  The  beam 
is  mounted  at  the  hub  of  the  motor  almost  rigidly.  In  the  mathematical  analysis, 
however,  the  stiffness  of  this  joint  is  taken  into  account.  A  collocated  tachometer 
and  an  optical  encoder  measure  the  shaft  angular  velocity  and  displacement. 
The  tip  reaction  force  is  measured  using  a  noncollocated  force  sensitive  resistor 
(FSR),  manufactured  by  Interlink  Electronics.  The  FSR  element  exhibits  an 
electrical  resistance  which  decreases  with  increasing  normal  pressure.  Unlike 
strain  gauges  the  change  of  FSR  resistance  is  extremely  large  and  follows  a  power 
law  relationship.  In  terms  of  the  time  response  the  FSR  can  be  considered  a 
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+15V 


Figure  2.1:  FSR  interface  circuit 

slow  device  (unlike  piezoelectric  transducers)  with  typical  mechanical  rise  time 
of  1-2  msec. 

For  a  simple  force-to-voltage  conversion,  the  FSR  is  tied  to  a  measuring 
resistor  in  a  voltage  divider  configuration.  The  FET-input  type  op-amps  LF 
355  isolate  the  output  signal  from  the  high  source  impedance  of  the  voltage 
divider  and  allow  adjustment  of  the  output  offset  and  gain.  The  FSR  interface 
circuit  is  presented  in  Figure  2.1.  The  values  of  R5  and  R$  are  40 ATI,  10 ATI 
and  10 ATI  respectively.  A  more  detailed  description  of  the  electronics  which  are 
installed  for  measuring  angular  position  and  velocity  of  the  motor  shaft,  can  be 
found  in  [1]. 
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2.2  The  beam  and  the  contact  model 


The  Euler-Bernoulli  beam  equation  governing  a  flexible  beam  is  a  partial  dif¬ 
ferential  equation  for  a  function  of  time  and  position  along  the  beam  and  is 
thus  a  distributed  parameter  system.  One  method  for  solving  such  systems  is  to 
approximate  the  beam  by  a  finite  number  of  rigid  elements  which  are  connected 
by  means  of  revolute  joints.  Posbergh,  in  [17]  has  shown  that  as  the  number 
of  elements  in  the  approximation  approaches  infinity,  each  solution  converges  to 
that  of  the  continuum  model. 

The  equations  for  the  finite  dimensional  system  constitute  a  Galerkin  model 
of  the  beam.  The  model  used  to  approximate  the  continuum  model  in  our  case 
was  a  three  body  approximation:  one  rigid  body  for  the  motor  and  the  hub 
and  two  rigid  bodies  for  the  flexible  beam.  The  rigid  bodies  are  connected  by 
rotary  joints  and  coupled  with  torsional  springs  which  depend  linearly  on  the 
angle  difference  between  the  bodies.  Let  fc,  be  the  spring  constant  of  the  iih 
joint.  The  center  of  mass  of  the  ith  body  of  the  beam  which  has  length  /,  lies  at 
a  point  e,  from  the  previous  hinge.  This  rigid  body  is  characterized  by  its  mass 
m,  and  the  moment  of  inertia  /,  about  its  center  of  mass. 

Two  methods  for  obtaining  the  equations  of  motion  are  :  the  Lagrangian 
and  the  Newton-Euler  formulations.  Here,  we  made  use  of  the  second  method 
which  is  more  suitable  since  it  deals  directly  with  external  torques  and  forces 
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m  0 

i  i 

Figure  2.2:  Finite  Element  Approximation 


while  the  Lagrangian  approach  does  so  somewhat  indirectly. 

As  far  as  the  contact  model  is  concerned,  we  assume  rigid,  point  frictionless 
contact.  The  collisions  taking  place  at  the  beam  tip  are  ideally  plastic  (coefficient 
of  restitution  e  =  0).  Therefore,  only  an  unknown  reaction  force  will  be  present 
at  the  point  of  interaction  instead  of  a  general  wrench.  Moreover,  given  the 
flexibility  and  the  dimensions  of  the  beam,  we  may  assume  that  the  line  of 
action  of  the  reaction  force  /  remains  always  normal  to  the  third  body  of  the 
finite  element  approximation. 

If  l2  and  /3  are  the  angular  momenta  of  the  second  and  the  third  rigid  bodies 
about  the  center  of  mass  and  the  arbitrary  torques  and  forces  'r1,r2,/1,/2  are  as 
in  Figure  2.3,  then 

I2  =  rl  +  r2  T  1  x  f*2  (2.1) 
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Figure  2.3:  Torques  and  forces  for  the  Newton-Euler  approach 

i3  —  ~t2  T  l  x  (— ^2)  +  7  f  (2-2) 

where  7  is  the  tip  position  vector  [l(cos03  +  cosO2),l(sin03  +  sin02, 0]r,  in  the 
reference  coordinate  system  (of  the  first  rigid  body)  and  1  is  the  position  vector 
that  corresponds  to  the  second  joint.  From  (2.1)  and  (2.2)  by  addition  we  get 

ri  =  ^2  +  I3  —  7  x  (2-3) 

In  the  general  case  the  Newton-Euler  formulation  gives  1  which  represents  a 
torque  as 

lj  —  Bjlifl,-  T  Bjlifl,  ■)■  m,(roi  x  Tqj) 

where  lj  is  the  inertia  tensor  of  the  iih  body  about  the  center  of  mass  expressed 
in  the  reference  frame,  fij  is  the  rotational  velocity  expressed  in  the  body  frame 
and  Bj  is  the  homogenous  transformation  from  the  ith  body  frame  to  the  base 
frame.  Also  r0j  is  the  position  vector  of  the  center  of  mass  of  the  ith  body  in 
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the  reference  frame.  Then  12  and  13  are  given  by 


with 


12  =  b2i2o2  +  b2i262  +  m2(ro2  x  ^02) 

13  —  63X303  -f  B3I363  +  m3(ro3  x  ro3) 


B2  = 


b3  = 


cos&2  —sin62 
sin02  cosd2 
0  0 

cos03  —sin03 
sin03  cos03 
0  0 


0 

0 

1 

0 

0 

1 


O2  =  [O,O,02]t 


(2-4) 

(2.5) 


n3  =  [o,o,03]T 


r02  =  [t2cos82:t2sin82,0]T 


r03  =  [ lcos02  +  e3cos03,  lsind2  +  e3sin&3, 0]T. 

Then  the  z-components  (torques  around  the  z-axis)  of  12  and  I3  are 

kz  =  hh  +  "*2^2  (2-6) 

i3iZ  =  (m3l2  +  m3le3cos(03  -  02))02  +  (I3  +  m3el  +  ^3Ie3cos(S3  ~  02))03 

•f  m3le3sin(03  -  02)(0%  -  0\)  (2.7) 
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Now  noticing  that  f  =  [fsin03,—fcos03, 0]r  where  /  is  the  amplitude  of  the 
external  force, 

f  x  7  =  [0, 0,  fl(\  +  cos{03  -  02)]t. 

If  in  (2.3)  we  include  the  torques  arising  from  the  springs  then 

k2{03  -  02 )  -  M#2  -  01 )  =  [m2e2  +  m3l2  +  /2  +  m3le3cos(03  -  02)]02 

+[m3e23  +  I3  +  m3le3cos(03  -  02)}03 
-m3le3sin(0 3  -  62){923  -  922) 

-f//[l  +  cos[93  —  #2)]. 

For  the  third  rigid  body  we  have 

t2  =  ~ I3  +  1  x  (— ' ^2)  +  7  x  f  (2-8) 

Here  we  are  making  use  of  the  Newton’s  equation  for  the  third  rigid  body  in 
order  to  derive  an  expression  for  f2 

—  f2  +  f  =  1^3*03  (2-9) 

then  (2.8)  becomes 

t2  =  -i3  +  l  x  (m3f03  -  /)  +  7  x  f 

—  — 13  +  1  x  m3?o3  +  (7  —  1)  x  f  (2.10) 

with  (7  —  1)  =  [lcos03,  lsin03, 0]T.  Including  again  the  torsional  terms  we  get 

-k2(63  -  62)  =  m3le3cos(03  -  02)}02  +  [m3c*  +  I3]03  +  m3lt3sin(93  -  92)9D  +  fl 
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If  the  motor  provides  T,n(f)  torque  on  the  shaft  then  easily 


IU0  =  M*  2-0.). 

The  above  equation  has  proved  to  be  inconsistent  with  the  experimental  data, 
so  it  was  updated  using  a  comparatively  accurate  friction  model  for  the  motor 
which  ta,kes  care  of  damping  and  kinetic  friction, 

Tin(t)  =  JA  -  kt(e2  -  6,)  +  kfA  +  KtsgnA).  (2.11) 

For  the  motor  the  following  equations  hold 

T.M)  =  MO  (2-12) 

v(t)  —  kBd  i 

Rai(t)  +  kB6^  =  u(t)  =»  i(t )  =  - - - ,  (2.13) 

Ka 

thus 

TM  =  TT«(0  -  (2.14) 

where  /Za  is  the  motor  armature  resistance,  kB  is  the  motor  back  EMF  constant 
and  kT  is  the  motor  torque  constant.  Summarizing,  the  nonlinear  equations  of 
motion  that  describe  the  constrained  motion  of  the  flexible  arm,  viewing  the 
system  with  input  as  voltage  rather  than  torque,  are  the  following: 

At)  =  /,».-  - «.)  +  M.  +  (2.i5) 


^2(^3 —  ^2) —  ^1(^2  —  ^1)  — 
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[m2el  +  m3l2  +  I2  +  m3k3cos(03  -  02)}02 
+[m3e3  +  h  +  rn3lt3cos(03  -  02)]03 

-m3k3sin(0 3  -  62){(>l  -  022)  +  //[  1  -f  cos(03  -  02)]  (2.16) 

-k2(63  -  02)  =  m3le3cos(03  -  02)02  4-  ( m3el  +  I3)03 

+m3lc3sin(03  -  02)0]  +  fl  (2.17) 


2.3  The  physical  parameters  of  the  system 


Before  proceeding  with  numerical  and  the  experimental  results,  let  us  first  de¬ 
scribe  the  system’s  physical  characteristics.  The  hub  mass  and  inertia  are 
computed  from  the  hub’s  dimensions.  The  hub  has  radius  r  =  11cm,  height 
h  =  0.87cm  and  it  is  made  of  aluminum  which  has  density  26.6  x  103iV/m3. 
Thus,  the  mass  density  is  pAl  =  26.6  x  103/9.806  =  2.71  x  10 3/\g,/m3.  The 
total  mass  is  computed  as  mj  =  pAlTrr2h  =  0.86 Kg  and  its  inertia  is  computed 
as  /]  =  mjr2/ 2  =  5.2  x  10 ~3Kgm2.  The  mass  of  the  second  and  the  third 
link  are  computed  from  the  mass  density  and  the  physical  dimensions  of  the 
beam.  The  beam  has  width  w  =  0.30625cm,  height  h  =  4.826cm  and  length 
L  =  lm.  Therefore,  m2  =  m3  =  0.20054 Kg.  The  inertia  of  the  approximating 
rigid  bodies  of  the  beam  is  given  by 


h  =  h  = 


PAlhw 

24 


=  1.67  x  10-5 
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kB 

2.443V /  (rad  /  sec) 

kj1 

0.2A9092Kgm/A 

Ra 

33.60 

kfr 

5.4192  x  10~3Nm/(rad/sec) 

k,t 

0M5Nm 

Table  2.1:  Physical  parameters  of  the  DC  brushless  motor 

The  stiffness  of  the  first  torsional  spring  between  the  hub  and  the  beam  is 

Ibin  Nm 

jfcj  =  1.5  x  106 — -  =  1.5  x  106  x  9. 806(1/2. 2046)(1  /39.37)  =  169468.0882 - 

rad  rad 

The  stiffness  k2  of  the  beam  was  found  to  be  k2  =  A.92Nm/rad  (see  [17]). 

Given  the  dimensions  of  the  beam  and  assuming  uniform  mass  distribution, 

the  Galerkin  model  parameters  t2  and  e3  are  both  taken  equal  to  0.25m.  The 

physical  parameters  of  the  DC  brushless  torque  motor  are  presented  in  Table 

2.1. 

Note  also  that  in  the  actual  system  there  exists  a  power  amplifier  unit  of  nominal 
gain  k  --  6.02.  The  input  to  this  unit  which  is  driven  by  the  PC  interface  has 
maximum  rating  ±10V. 

2.3.1  Empirical  system  response 

The  experimental  hardware  incorporates  an  optical  shaft  encoder  to  measure 
angular  displacement  and  a  tachometer  to  measure  the  angular  velocity  of  the 
motor  shaft,  i.e.  of  the  first  body  of  the  approximation.  No  sensors  are  available 
for  the  remaining  states  of  the  system.  Notice  that  given  the  high  stiffness 
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time  [sec] 

Figure  2.4:  Optical  encoder  output 

in  the  coupling  between  the  hub  and  the  beam,  the  states  (0,0,0)  of  the  first 
and  the  second  rigid  bodies  are  almost  identical.  Figures  2.4,  2.5  and  2.6  show 
sample  profiles  from  the  optical  encoder,  the  tachometer  and  the  force  transducer 
respectively  for  a  continuous  input  of  10%  of  the  maximum  torque  of  the  motor. 
The  impact  takes  place  at  O.lrad  angle  from  the  reference  position.  We  assume 
that  the  beam  collides  with  a  single  rigid  body  of  infinite  mass  such  as  a  rigid 
wall.  The  absolute  velocity  of  this  body  remains  zero  throughout  the  experiment. 
The  figures  clearly  indicate  the  various  phases  of  the  sample  experiment: 

•  Motion  in  free  space  (reaction  force  is  zero). 

•  Time  instant  of  initial  contact  (instantaneous  velocity  increments). 

•  Discontinuity-free  impact  response  which  depends  on  the  system  dynamics. 

•  Loss  of  contact  -  bouncing. 

•  Steady-state  phase  (application  of  some  constant  tip  force). 
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Figure  2.5:  Tachometer  output 

The  force  output  profile  is  to  be  understood  as  being  more  or  less  qualitative 
between  the  time  instants  of  initial  contact  and  steady-state  phase,  since  the 
FSR  (Force  Sensing  Resistor)  sensor  is  unable  to  respond  to  this  kind  of  high- 
frequency  excitation.  The  use  of  a  completely  different  type  of  sensor  such  as  a 
piezoelectric  sensor,  and  some  noise  reduction  circuitry  might  improve  the  sensor 
output.  However,  we  should  keep  in  mind  that  the  output  of  most  common  force 
transducers,  including  piezoelectric  and  strain-gauge  type,  goes  off-scale  in  the 
impact  phase.  Consequently,  the  force  output  during  this  time  period  does  not 
convey  dependable  information  to  be  used  for  feedback. 

The  above  profiles  are  available  for  comparison  with  the  numerical  results 
that  follow  in  the  next  chapter. 
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time  [sec] 
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ure  2.6:  FSR  output 
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Chapter  3 

Model  validation  using  a  numerical 
approach 

3.1  Description  of  the  numerical  problem 

The  system  of  equations  (2. 15), (2. 16), (2. 17)  that  describes  the  motion  of  the 
beam  is  nonlinear  and  closed  form  analytical  solutions  are  unobtainable.  It  is 
therefore  necessary  to  obtain  suitable  numerical  approximations  to  the  unknown 
angular  displacements,  velocities  and  accelerations.  The  Newmark  family  of  in¬ 
tegration  methods  is  perhaps  the  most  widely  used  family  for  solving  this  type 
of  equations  describing  the  dynamics  of  rigid  body  assemblies.  The  Newmark 
family  contains  as  special  cases  several  well-known  numerical  schemes.  There¬ 
fore,  the  next  section  is  devoted  to  a  review  of  the  Newmark  algorithm.  A  more 
detailed  description  of  the  algorithm  can  be  found  in  [15]. 
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3.1.1  The  Newmark  approximation 


We  assume  that  the  dynamic  behavior  of  a  rigid  body  system  may  be  written 
as  a  family  of  n  differential  equations  given  by 

Mx(t)  +  P(x(t),x(t))  =  F(t),  x(0)  =  x0,  i(0)  =  x0 

where  x(t)  is  a  N  x  1  vector  of  system  displacements,  M  is  an  TV  x  TV  mass-type 
matrix,  P  is  a  general  TV  x  1  vector  depending  on  velocities  and  displacements 
of  the  system  and  F(t)  is  an  TV  x  1  vector  of  forces  applied  at  the  global  degrees 
of  freedom. 

The  Newmark  integration  method  approximates  the  response  of  nonlinear 
2-nd  order  equations  by  making  the  assumption  that  the  balance  laws  should 
be  satisfied  at  every  discretization  instant.  Moving  to  discrete  dynamics  for  two 
consecutive  time  steps  we  have 

Mxn  +  P(x(tn),x(tn))  =  Fn  (3.1) 

Mxn+1  +  P{x(tn+ !),a:(tn+1))  =  Fn+1.  (3.2) 

Now  assume  that  (3.1)  has  already  been  solved.  We  are  interested  in  solving 
(3.2)  for  t  G  [tn,tn+ 1]  given  the  state  (xn,in,in)  as  the  initial  condition.  Since 
the  number  of  equations  is  N,  while  the  number  of  the  unknowns  is  37V,  some 
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relationships  between  accelerations,  velocities  and  displacements  should  be  em¬ 
ployed  in  order  to  solve  the  problem.  In  general 


x(t)  =  x(tn)  +  /  x(r)dT, 

J  tn 

t  ^  [^ni  ^n+l] 

x(t)  =  x(tn)  +  l  x(t )dr, 

Jtn 

t  e  [<*,<n+l] 

The  main  point  of  the  Newmark  method  is  that  it  makes  reasonable  assumptions 
about  the  change  of  the  acceleration  between  the  discretization  time  instants, 
while  insisting  on  maintaining  equilibrium  at  tn  and  tn+1.  There  are  several 
possible  implementations.  For  example,  assume  that  the  acceleration  vector 
x(t)  is  a  linear  combination  of  the  boundary  values  x(tn )  and  x(tn+1), 

x(t)  =  (1  -  7)x(<n)  +  'yx(tn+1 ),  t  E  [tn,  tn+ 1]  (3.3) 

where  7  is  some  prescribed  constant.  From  the  above  we  obtain 

x(t)  =  x(tn)  +  [(1  -  j)x{tn)  +  'ix{tn+1)](t  -  tn)  (3.4) 

x(t)  =  x{in)  +  -  tn)  +  [{}  -7)i(U  +  7i(*n+i)]^y “  (3-5) 

Substituting  the  above  equations  in  (3.2)  we  end  up  with  a  system  of  N  equa¬ 
tions  with  N  unknowns,  namely  the  vector  x(tn+1).  Recasting  x(tn+l)  into 
(3. 3), (3. 4), (3. 5)  the  state  of  the  system  is  completely  determined.  The  general 
Newmark  family  that  includes  the  above  case  is  summarized  below: 

x(tn+1)  =  ±(in)  +  h[(l  -  7)5(0  +  7*(*n+i)]  (3-6) 

*(<n+i)  =  *(0  + MU +  fc2(^  “£)*(*») +  fc20i(<»+i)»  (3.7) 
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where  h  is  the  time  step  (tn+1  —  tn).  Notice  that  instead  of  solving  for  the 
accelerations  for  the  next  time  step,  we  can  solve  for  either  displacements  or 
velocities  once  the  relationships  between  the  three  vectors  h'ave  been  established. 

The  determination  of  the  truncation  errors,  the  accuracy  of  the  method  and 
the  stability  matters  are  beyond  the  scope  of  this  review  and  they  can  be  found 
in  [15].  However,  we  recall  some  key  results. 

Remark:  The  stability  and  the  accuracy  of  the  method  depends  on  (3  and 
7  parameters  and  on  time  step  as  well.  A  summary  of  the  stability  conditions 
for  the  general  Newmark  method  is  given  in  Table  3.1. 


Unconditional 

2/3  >  7  >  1/2 

Conditional 

7  >  1/2,  ^  <  7/2 ,uhh  < 

Table  3.1:  Stability  conditions  of  Newmark  method 


u>h  is  the  maximum  natural  frequency  of  the  system  and  Hcrt(  is  the  criti¬ 
cal  sampling  frequency  given  by  f2CTif  =  (7/2  —  /3)_1/2  for  the  undamped  case. 
Therefore,  the  trapezoidal  rule  (/?  =  0.25,7  =  0.5)  is  unconditionally  stable 
while  the  linear  acceleration  method  (/3  =  1/6,7  =  0-5)  is  conditionally  stable 
with  f 1  ~  3.464.  The  accuracy  of  these  methods  is  of  2nd  order  provided  7  =  0.5. 

Of  great  interest  is  the  high  frequency  behavior  of  the  Newmark  method, 
since  higher  modes  are  artifacts  of  the  impact  phenomena.  Therefore,  it  is  gen¬ 
erally  viewed  as  desirable  and  often  it  is  considered  necessary  to  have  some  form 
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of  algorithmic  damping  present  to  remove  the  high  frequency  components.  For 
the  Newmark  method,  7  >  0.5  is  necessary  to  introduce  high  frequency  dissipa¬ 
tion.  Also  for  some  fixed  7,  a  particular  value  of  /?  maximizes  the  dissipation. 
Unfortunately  7  ^  0.5  results  in  a  drop  to  1st  order  accuracy. 


3.1.2  Numerical  solution  of  the  unconstrained  motion  of 
the  beam 


The  equations  of  motion  (a  system  of  ordinary  differential  equations)  are  dis¬ 
cretized  using  the  above  described  Newmark  algorithm.  The  nonlinear,  algebraic 
system  of  equations  that  arises,  has  to  be  solved  for  either  accelerations,  veloc¬ 
ities  or  displacements.  There  are  no  good  general  methods  for  solving  systems 
of  more  than  one  nonlinear  equation.  Almost  always  we  have  to  use  additional 
information  specific  to  the  particular  problem  to  answer  questions  about  unique¬ 
ness  etc.  However,  if  we  identify  a  neighborhood  of  a  root,  the  Newton-Raphson 
method  gives  a  very  efficient  means  of  converging  to  the  root,  if  it  exists,  pro¬ 
vided  a  good  initial  guess  was  used.  Generally  the  object  is  to  determine  a  vector 
x  =  [xj,a:2,  ...xn]  such  that  the  functions  /i(x1,x2,  ...xn),  where  i  —  1,2,  ...n,  are 
approximately  zero.  In  the  neighborhood  of  x  each  function  /,  can  be  expanded 
in  Taylor  series: 


df i 


/.(x  +  6x)  =  /t(x)  +  +  0{6x2). 


3=1 
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By  neglecting  terms  of  <f>x2  order  and  higher  we  obtain  a  set  of  linear  equations 
for  the  corrections  8x  that  force  the  functions  /,(x  +  <5x)  to  be  zero  simultane¬ 
ously: 

J<5x  =  —  f(x),  (3.8) 

where  J  is  a  square  matrix  of  partial  derivatives  (df{/dxj)  and  is  called  the 
Jacobian.  Now  (3.8)  is  a  linear  system  of  equations  which  can  be  easily  solved 
given  that  det(J)  ^  0.  The  corrections  are  then  added  to  the  solution  vector: 

xnew  =  xold  +  fa 

and  the  process  is  iterated  to  convergence. 

The  routine  mnewt(ntrial,x,n,  tolx,tolf)  of  the  package  Numerical  Recipes 
[21]  performs  this  job  for  ntrial  iterations  starting  from  an  initial  guess  x.  Here 
n  is  the  order  of  the  system  and  tolx  and  tolf  are  tolerances  for  a  stopping  rule. 

The  above  tools  (the  Newmark  approximation  along  with  the  Newton  Raph- 
son  method)  were  employed  to  solve  the  nonlinear  system  of  equations  (2.15), 
(2.16),  (2.17)  where  /  =  0  for  the  unconstrained  case.  Initially,  the  well  known 
trapezoidal  rule  was  proven  to  be  sufficient  to  describe  the  free  motion  of  the 
beam  in  terms  of  displacements  and  velocities.  However,  some  rearrangement 
of  (}  parameter  was  required  for  high  frequency  dissipation  in  the  blurry  and 
poor  acceleration  profiles.  Velocity  and  displacement  plots  remained  the  same 
for  various  ft  within  the  stability  region  2/9  >  7.  Values  of  7  =  0.5  and  ft  ~  0.4 
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were  considered  suitable  throughout  the  numerical  experiments.  This  value  of  7 
maintains  the  2nd  order  accuracy  even  though  it  does  not  provide  high  frequency 
dissipation.  Time  steps  of  the  order  of  10_2sec  up  to  10-3sec  are  adequate  for 
the  unconstrained  motion. 

Remark:  In  using  the  Newton-Raphson  method  to  solve  the  nonlinear  sys¬ 
tem,  we  note  that  a  good  initial  guess  for  the  angular  displacement  vector  is 
dictated  by  the  angular  displacements  of  the  previous  time  step.  As  soon  as 
the  beam  follows  a  continuous  trajectory  with  no  impact  phenomena  present, 
the  state  of  the  previous  time  step  should  always  lie  in  the  neighborhood  of  the 
current  state.  The  smaller  the  time  step  h  gets,  the  smaller  the  difference  A0 
from  the  previous  time  step  and  the  higher  the  degree  of  convergence  to  the 
root. 

3.1.3  Numerical  results  for  the  constrained  motion  of  the 
beam 

By  the  time  contact  is  established  at  the  tip  of  the  beam,  one  more  unknown, 
namely  the  amplitude  of  the  reaction  force,  is  present.  Therefore,  one  more 
equation  must  be  enforced.  Recall  that  a  contact  problem  is  actually  a  con¬ 
straint  problem.  The  primary  kinematic  axiom  of  a  contact  problem  is,  that  the 
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Figure  3.1:  Configuration  of  the  beam  in  contact 

structures  involved  do  not  penetrate  each  other.  Figure  3.1  shows  the  configu¬ 
ration  of  the  beam  in  two  consecutive  states  while  contact  is  established1.  The 
kinematic  constraint  is  determined  by  the  fact  that  the  contact  point  should 
always  belong  to  the  line  aligned  to  the  third  rigid  body.  If  the  coordinates  of 
the  contact  point  are  (a,  b)  then  the  numerical  kinematic  constraint  is  given  by: 

lsin63  b  —  lsind2 

3  lcos03  a  —  lcos02 


which  implies, 


asin03  —  bcos03  —  lsin(63  —  02)  —  0. 


(3.9) 


Therefore,  the  constrained  motion  of  the  beam  is  described  by  the  dynamic 

^ote  here,  that  the  contact  point  moves  with  respect  to  the  coordinate  frame  of  the  third 
body  of  the  approximation.  However,  the  travel  of  the  contact  point  is  negligible  for  the  given 
beam  flexibility.  Additionally,  recall  that  this  motion  is  assumed  to  be  frictionless. 
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equations  (2.15),  (2.16),  (2.17)  and  the  algebraic  kinematic  constraint  (3.9).  Us¬ 
ing  again  the  Newmark  approximation  with  7  =  0.5  and  0  =  0.4  along  with  the 
Newton-Raphson  method,  we  are  moving  back  and  forth  from  a  3-dimensional 
system  of  equations  to  a  4-dimensional  one.  The  transitions  are  primarily  de¬ 
termined  by  the  kinematic  constraint.  Additional  numerical  constraints  correct 
possible  truncation  errors.  The  first  results  of  this  method  were  consistent  as 
far  as  displacement  and  velocity  were  concerned,  but  this  was  not  the  case  for 
the  acceleration  and  force.  Moreover,  force  plots  were  affected  by  the  time  step 
changes.  Notice  that  we  already  expected  a  poor  acceleration  profile  since  the 
Newmark  approximation  for  the  acceleration  variation  inside  the  time  steps  was 
used.  The  method  worked  sufficiently  well  after  a  good  choice  of  0  for  the  free 
motion,  but  here  high-frequency  impact  phenomena  are  involved.  The  velocity 
and  displacement  plots  of  the  first  and  the  third  body  are  presented  in  Figures 
3.2  and  3.3.  The  parameters  used  are  time  step  of  0.001  sec,  0  =  0.4,  7  ==  0.5 
and  constant  input  voltage  of  IV.  Bear  in  mind  that  given  the  high  stiffness 
coupling  between  the  hub  and  the  beam,  similar  plots  for  the  second  body  of 
the  Galerkin  approximation  hold. 

Generally,  force  and  acceleration  are  closely  related  therefore  ,  any  problems 
in  the  acceleration  plots  will  be  transferred  to  the  force  plot.  This  can  be  also 
seen  as  follows.  Let  us  take  the  integral  in  equation  (2.17)  from  the  moment 
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Figure  3.2:  Velocity  and  displacement  profile  of  the  first  body  using  step  of 
O.OOlsec,  f3  =  0.4,  7  =  0.5  and  voltage  input  IV 
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Figure  3.3:  Velocity  and  displacement  profile  of  the  third  body  using  step  of 
O.OOl^ec,  /3  —  0.4,  7  =  0.5  and  voltage  input  IV 
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just  before  impact  t0  up  to  t0  +  At  and  let  At  go  to  zero. 


rto+nt  rto+at  yto+m 

-  /  k2(63  -  02)dt  =  /  fldt  +  /  m3lt3cos(d 3  -  02)02di 

J  to  ^  ^0  •'to 

rto-fAt  ^  y*to-fAt 

+  /  [m3e2  +  /3]03<ft  +  /  m3le3sin(63  -  62)622)dt 

J to  J to 


(3.10) 


As  it  will  be  shown  in  the  next  chapter,  during  the  short  time  interval  that 
impact  lasts,  the  position  of  all  rigid  bodies  remain  unchanged  since  all  angular 
velocities  remain  finite.  Springs,  dampers  and  similar  elements  in  the  hinges  do 
not  play  any  role  since  they  exert  wrenches  of  finite  magnitude.  Only  impulsive 
forces  and  torques  can  cause  instantaneous  velocity  increments. 


Definition:  A  force  f(t)  is  called  an  impulsive  force,  if  the  integral 


converges  to  a  finite  quantity  /  when  At  tends  to  zero. 


Therefore,  from  equation  (3.10)  above  we  get 

rfo+At  ^  rto  +  At 

0  -m3lt3cos(03  -  02)  92dt  +  [m3t\  +  I3\  /  §3dt  +  fl  (3.11) 

Jt0  Jto 

(the  02  term  remains  finite  so  that  in  the  limit  the  integral  is  zero)  this  means 
that  during  impact,  accelerations  and  forces  are  closely  related.  A  more  detailed 
analysis  of  impact  problems  in  multi-body  systems  presented  in  the  next  chapter, 
clarifies  the  above  assertions. 


Even  ill 
the  force. 


ough  we  do  not  care  too  much  about  accelerations,  we  do  care  about 
A  first  handy  solution  for  improving  the  force  plot  is  to  introduce 
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high  frequency  dissipation  by  forcing  7  >  1/2.  This  pushes  the  system  in  the 
conditional  stability  region  and  reduces  accuracy  to  1st  order.  Therefore,  a 
smaller  time  step  h  should  be  picked.  A  program  that'  scans  the  spectrum 
of  possible  values  for  (3  and  7  while  keeping  the  time  step  under  reasonable 
limits  (e.g.10-5)  was  developed.  Under  these  conditions  no  pair  of  (3  and  7  gave 
acceptable  stable  results. 

Note  that  high  frequency  problems  arise  not  only  from  the  Newmark  method, 
but  also  from  the  numerical  differentiation  in  the  Newton-Raphson  solution.  It  is 
well  known  that  the  Newton-Raphson  method  will  spectacularly  fail  to  converge 
if  the  initial  guess  is  not  good  enough.  And  this  is  the  case  here  since  force 
experiences  sudden  changes  in  short  periods  of  time. 

However,  given  that  the  Newmark  method  is  quite  efficient  for  angular  veloc¬ 
ities  and  displacements  as  it  is  established  by  comparisons  with  the  experimental 
data  (compare  Figures  2.4  and  2.5  with  Figure  3.2),  another  method  is  proposed 
here  in  order  to  find  the  true  accelerations  and  the  reaction  force. 

Strategy:  At  each  time  step  while  contact  is  established  we  allow  two  dif¬ 
ferent  numerical  runs.  The  first,  using  the  Newmark  approximation  along  with 
the  Newton-Raphson  method  for  solving  the  nonlinear  system  that  arises,  pro¬ 
vides  the  vectors  6  and  6.  With  the  second  running,  we  recast  6  and  6  back  in 
the  constrained  system  of  equations  (2.15),  (2.16),  (2.17).  Taking  the  derivative 
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with  respect  to  time  of  the  kinematic  constraint  equation  twice,  we  get 

lcos(03  —  d2)02  +  (acosd  3  -f  bsin03  —  lcos(d3  —  62))63  =  0  (3.12) 

and 

lcos(03  —  02)02  +  (acos6  3  +  bsin63  —  lcos(63  —  02))§3  =  2  lsin(63  —  02)0203 
—  lsin(d3  —  O2)0 j  —  (—asind  3  +  bcos03  +  lsin(03  —  62))6 ^  (3.13) 

Recall  that  during  the  infinitesimally  short  time  interval  that  impact  lasts, 
the  positions  of  all  rigid  bodies  do  not  change  since  all  velocities  remain  finite. 
Therefore,  displacements  are  continuous  functions  of  time  and  there  is  no  prob¬ 
lem  present  in  the  first  differentiation.  However,  the  impulsive  reaction  force 
present  at  the  tip  can  cause  instantaneous  velocity  increments  of  finite  magni¬ 
tude.  One  should  keep  in  mind,  that  at  least  theoretically  acceleration  reaches 
infinity  at  the  time  instant  of  initial  contact. 

Now,  at  each  time  step  during  contact,  the  acceleration  vector  6  and  the 
reaction  force  should  satisfy  (2.15),  (2.16),  (2.17)  and  (3.13)  as  well.  Therefore, 
we  obtain  a  linear  system  of  equations  with  unknowns  0  and  /.  A  simple  matrix 
inversion  solves  the  problem.  The  algorithm  is  summarized  in  Table  3.2,  while 
the  corresponding  C  code  can  be  found  in  the  Appendix.  The  results  are  pre¬ 
sented  in  Figures  3.4  and  3.5  where  a  continuous  input  of  10%  of  the  maximum 
torque  of  the  motor  is  applied.  The  impact  takes  place  at  0.1  rad  angle  from  the 
equilibrium  position  just  as  in  the  experimental  case  (see  Chapter  2). 
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1  initialize  displacement,  velocity  and  acceleration  vectors 

2  if  contact  was  not  established  in  the  previous  time  step,  check  if 
the  current  displacement  vector  satisfies  the  contact  condition  (3.13) 

2.1  if  the  condition  is  still  not  satisfied  solve  the  3-dim  nonlinear 
system  that  arises  from  the  Newmark  approximation 

and  update  the  vectors 

2.2  if  the  condition  is  satisfied,  solve  the  4-dim  nonlinear  system 
(using  the  kinematic  constraint);  substitute  the  resulting  velocity 
and  displacement  vectors  back  to  the  initial  equations  and  solve  the 
linear  4-dim  system  (using  the  second  derivative  of  the 
kinematic  constraint);  get  the  stable  acceleration  vector  and 
reaction  force;  update  vectors 

3  if  contact  was  already  established  in  the  previous  step,  check 

if  the  current  displacement  vector  satisfies  the  release  condition 

3.1  if  contact  is  still  satisfied  follow  the  procedure  of  step  2.2 

3.2  if  release  condition  is  satisfied  follow  the  procedure  of  step  2.1 

4  repeat  for  the  next  time  step 

Table  3.2:  Summary  of  the  algorithm  predicting  the  constrained  beam  motion 


Figure  3.4:  Reaction  tip  force  profile  using  step  of  O.OOlsec,  /3  =  0.4,  7  =  0.5 
and  voltage  input  IV 
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Figure  3.5:  Acceleration  profile  of  the  third  body  using  step  of  0.001  sec,  ft  =  0.4, 
7  =  0.5  and  voltage  input  IV 

Time  steps  of  the  order  10-3  or  smaller  are  suggested  for  simulating  the 
constrained  motion  of  the  beam.  If  this  is  the  case,  the  numerical  and  the 
experimental  results  are  surprisingly  close  given  that  the  physical  characteristics 
of  the  system  and  the  position  where  the  impact  happens  may  include  some 
errors. 

Some  quick  observations  can  be  made.  A  number  of  pulses  in  the  reaction 
force  precede  the  steady-state  force.  The  beam  bounces  before  it  settles  at  the 
equilibrium  position.  The  number  of  pulses  and  their  duration  depends  mainly 
on  the  kinetic  energy  that  is  accumulated  in  the  system  (and  consequently  on 
the  constant  motor  torque),  as  well  as  on  the  beam  stiffness.  In  the-  limiting 
case  where  there  is  no  flexibility  at  all,  these  pulses  converge  to  delta  functions, 
making  the  impact  phenomenon  impossible  to  control.  Therefore,  some  amount 
of  flexibility  is  deliberately  desirable  not  only  because  it  provides  the  system 
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with  the  necessary  compliance  to  touch  objects,  but  also  because  it  grants  the 
impact  force  controller  with  additional  time  to  respond. 

The  maximum  force  occurs  when  the  angular  velocities  of  all  bodies  reach 
zero.  From  various  experiments  that  were  conducted,  this  maximum  force  seems 
to  depend  on  the  velocities  just  before  impact,  a  factor  that  has  found  wide  use 
in  impact  and  force  control  in  the  past  ([27]).  Unfortunately,  this  qualitative 
observation  is  insufficient  to  solve  the  impact  controller  design  problem  since 
it  ignores  the  dynamics  of  the  impact,  i.e.  the  type  of  the  contact  and  the 
structure  of  the  multi-body  system  that  collides.  More  details  will  follow  in  the 
next  chapter.  For  the  sake  of  comparison,  Figures  3.6-3.11  present  the  results 
of  the  method  for  various  time  steps  and  input  voltages. 

Remark  1:  we  do  not  claim  that  the  Newmark  approximation  cannot  solve 
the  constrained  motion  problem.  Simply,  since  high-frequency  phenomena  are 
involved,  unacceptably  small  time  steps  should  be  used  to  avoid  instability. 
Changing  the  value  of  7  to  introduce  high-frequency  dissipation,  a  drop  in  the 
order  of  accuracy  is  inevitable  as  well.  We  should  also  emphasize  that  more 
sensitive  impact  and  release  conditions  are  necessary  if  one  is  to  avoid  taking 
excessively  small  time  steps. 

Remark  2:  the  numerical  results  presented  in  this  chapter  can  be  easily 
expanded  using  a  higher  order  finite  element  approximation.  As  we  noticed, 
the  kinematic  constraint  reduced  the  degrees  of  freedom  of  the  multibody  chain 
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Figure  3.6:  Velocity  and  displacement  profile  of  the  first  body  using  step  of 
O.Olsec,  0  =  0.4,  7  =  0.5  and  voltage  input  IV 

manipulator  by  one.  In  the  case  of  an  N-order  approximation,  we  obtain  N  +  1 
equations  (  the  additional  equation  refers  to  the  hub  body)  and  one  kinematic 
constraint  of  the  form  q(9f  ,  02,  ■■■0N+i)  =  0.  Using  the  algorithm  presented 
above,  although  the  numerical  complexity  of  the  problem  is  increased,  the  solu¬ 
tion  is  still  feasible  using  no  additional  assumptions. 
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Figure  3.7:  Reaction  tip  force  profile  using  step  of  O.Olsec,  /?  =  0.4,  7  =  0.5  and 
voltage  input  IV 
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Figure  3.8:  Acceleration,  velocity  and  displacement  profile  of  the  third  body 
using  step  of  O.Olsec,  /3  =  0.4,  7  =  0.5  and  voltage  input  IV 
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Figure  3.9:  Velocity  and  displacement  profile  of  the  first  body  using  step  of 
O.OOlsec,  /3  =  0.4,  7  =  0.5  and  voltage  input  5V 


Figure  3.10:  Reaction  tip  force  profile  using  step  of  O.OOlsec,  ft  =  0.4,  7  =  0.5 
and  voltage  input  bV 
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Chapter  4 

Prediction  of  the  maximum 
reaction  force 

4.1  The  maximum  force  prediction  as  an  advan¬ 
tage  of  the  impact  controller 


In  the  next  chapter  where  the  design  problem  of  the  impact-force  controller  is 
addressed,  we  shall  see  the  importance  of  the  ability  to  estimate  the  maximum 
reaction  force1  ahead  of  time.  Obviously  the  most  accurate  way  to  proceed,  is  to 
solve  the  problem  numerically.  However,  as  was  demonstrated  in  the  previous 


'We  shall  see  in  this  chapter  that  a  typical  contact  force  profile  is  comprised  of  an 
initial  delta  function  (which  causes  the  instantaneous  velocity  increments),  followed  by  a 
discontinuity-free  portion.  The  importance  of  the  impulsive  part  is  completely  theoretical 
since  such  event  is  neither  physically  captured  nor  controlled.  In  the  sequel,  the  term  maxi¬ 
mum  reaction  force  refers  to  the  finite  portion  of  the  reaction  force. 
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chapters,  this  involves  comparatively  long  time  delays  for  real-time  implemen¬ 
tations.  In  recent  work  the  problem  of  finding  closed  form  estimates  of  the 
maximum  impact  force  was  based  exclusively  on  the  energy  principle  ([16], [7]). 
This  method  seems  to  work  sufficiently  well  for  the  simplified  case  of  single-body 
collisions  and  for  low-stiffness  contacts.  The  alternate  way  that  is  proposed  here 
is  based  on  understanding  of  the  impact  phenomenon  and  it  is  restricted  neither 
by  the  inherent  characteristics  of  the  bodies  involved  in  the  impact  problem,  nor 
by  the  complexity  of  the  body-structure.  The  general  questions  addressed  here, 
are: 

1.  Given  the  complete  state  of  the  system  ( 9 , 9 ,  0 )  and  some  specified  control 
law,  is  it  possible  to  get  an  estimate  of  the  maximum  impact  force  that 
will  be  developed  at  the  tip  if  contact  is  sensed  in  the  very  next  time  step? 

2.  What  is  the  time  interval  between  the  initial  contact  and  the  time  of 
maximum  reaction  force? 

If  we  can  answer  these  types  of  questions,  then 

1 .  we  can  avoid  entering  the  unstable  or  the  unable-to-respond  region  of  the 
impact  controller  (for  example,  by  reducing  the  impending  contact  velocity 
while  still  in  free  space)  and 

2.  using  a  trial  and  error  type  method  we  can  justify  the  control  law  that  the 
impact  controller  proposes  for  diminishing  the  impact  effect. 
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The  conventional  energy  method,  while  it  does  not  provide  answers  to  all  of 
the  above  questions  and  is  not  of  much  help  in  the  impact  controller  design,  will 
be  demonstrated  here  for  the  sake  of  comparison. 

4.1.1  Some  basic  assumptions  on  impact  dynamics 

The  present  subsection  studies  the  discontinuous  changes  of  velocities  and  an¬ 
gular  velocities  which  a  system  experiences  when  impact  takes  place.  Impact 
phenomena  occur  when  a  multi-body  system  collides  with  another  multi-body 
system.  The  physical  processes  during  impact  are  highly  complex  and  some  sim¬ 
plifying  assumptions  must  be  made  in  order  to  solve  the  mathematical  problem 
that  arises.  Whether  these  are  acceptable  must  be  judged  in  each  particular 
case.  Extensive  results  on  impact  problems  in  holonomic  multi-body  systems 
can  be  found  in  [26]. 

In  general,  the  impact  phenomenon  lasts  a  comparatively  short  time  inter¬ 
val  At,  usually  of  the  order  of  milliseconds  in  our  flexible  beam  experiment. 
Wittenburg  [26]  in  his  analysis  makes  the  assumption  that  in  the  mathematical 
description,  the  idealization  At  — *  0  is  possible.  This  implies  (and  is  implied) 
that  any  propagation  of  waves  of  deformation  through  bodies  is  neglected  since 
such  processes  require  finite  periods  of  time  to  develop.  Hence,  the  bodies  of  the 
system  are  treated  as  rigid  bodies  during  impact.  Since  Wittennburg’s  analysis 
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assumes  no  compliance  present  in  the  hinges,  the  interaction  between  the  col¬ 
liding  systems  is  completed  in  an  infinitesimal  amount  of  time.  This  is  certainly 
not  the  case  for  our  experiment.  Even  if  we  assume  rigid  collisions  at  the  point 
of  interaction,  the  flexibility  of  the  beam  spreads  the  impact  effect  in  time.  Delta 
functions  of  the  impact  force  profile  for  the  rigid  body  -  rigid  joint  case  become 
pulses  of  substantial  duration  for  flexible-body  systems.  Propagation  of  waves 
of  deformations  cannot  be  neglected.  But  the  analysis  presented  in  [26]  is  useful 
to  explain  phenomena  that  take  place  at  the  time  instant  of  initial  contact  (as 
At  — >  0),  e.g.  instantaneous  velocity  increments  etc.  The  dynamics  along  with 
the  kinematic  constraint  take  care  of  the  smooth,  discontinuity-free  motion  of 
the  beam  just  after  this  time  instant. 

In  this  preliminary  subsection  we  will  concentrate  only  on  what  happens  to 
the  system  over  the  infinitesimally  short  time  interval  just  before  and  just  after 
contact.  During  this  interval  the  positions  of  all  rigid  bodies  remain  unchanged, 
since  all  velocities  and  angular  velocities  remain  finite.  Springs,  dampers  etc, 
exert  forces  and  torques  of  finite  magnitude  whose  integral  over  this  infinitesi¬ 
mally  short  duration  is  zero.  As  was  already  mentioned,  only  impulsive  forces 
and  torques  can  cause  discontinuous  changes  of  velocities.  We  define  the  impulse 
as: 

rto+At 

f  =  f{t)dL 

An  impulsive  torque  similarly,  is  called  impulse  couple. 
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Figure  4.1:  Lumped  parameter  spring-mass  model  for  the  beam  experiment 
Now  depending  on  the  nature  of  collision,  elastic  and/or  plastic  deformation 
may  occur.  However,  we  allow  only  ideally  plastic  deformation  in  the  tip-object 
contact  (coefficient  of  restitution  e  =  0.0)  and  consequently  the  colliding  bodies 
have  zero  relative  velocity  at  the  point  of  collision  immediately  after  impact  as 
well  as  during  the  whole  period  that  contact  is  maintained.  This  is  because  the 
kinetic  energy  that  is  imparted  to  the  system  by  the  collision  is  relaxed  on  beam 
deformation  rather  than  on  contact  deformation.  For  convenience,  imagine  our 
system  as  springs  connected  in  series  as  in  Figure  4.1. 


1 

Kg 


where  i  —  2  in  our  case.  Since  the  stiffness  k1  is  the  largest,  the  beam  bending 
gets  virtually  all  the  kinetic  energy  .  If  we  still  insist  on  characterizing  the  type 
of  the  collision  through  some  coefficient  of  restitution  we  can  define 
kinetic  energy  released  during  restitution 

£  — - - — - - - - 7 - 

eq  elastic  energy  stored  during  collision  in  the  beam  sti f  fness 
where  ceq  <  1.  Therefore,  we  view  the  collision  as  having  clearly  separated 
compression  and  decompression  phases  which  refer  to  the  beam  stiffness  (not  on 
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Figure  4.2:  Body  configuration  during  collision 


a  stiffness  modeling  the  contact).  We  also  assume  that  the  impulsive  interaction 
forces  are  directed  normal  to  the  common  tangent  plane  at  the  point  of  collision. 
This  meams  that  there  is  no  friction  between  the  bodies  and  no  tangential  forces 
are  present.  Finally,  we  do  not  allow  any  play  in  the  hinges. 

4.1.2  Instantaneous  velocity  increments 

With  the  assumptions  specified  in  the  previous  subsection,  the  problem  of  impact 
is  now  reduced  to  the  determination  of  two  groups  of  mechanical  quantities. 
One  group  is  made  up  of  the  instantaneous  velocity  increments.  The  second 
group  comprises  external  impulses  at  the  point  of  collision.  In  order  to  get 
explicit  solutions  for  the  above  quantities,  the  kinematics  and  the  dynamics  of 
the  colliding  bodies  are  required.  For  the  present  purposes,  we  assume  that  the 
second  colliding  system  is  a  single  rigid  body  of  infinite  mass,  such  as  a  rigid  wall. 
The  absolute  velocity  of  this  body  remains  zero  throughout  the  experiment.  A 
general  impact  set-up  is  shown  in  Figure  4.2.  It  does  not  matter  to  how  many 
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contiguous  bodies  the  colliding  body  is  coupled.  After  all  hinges  are  cut,  some 
resulting  force  Xc  and  torque  Yc  act  on  the  terminal  body.  In  general,  Xc  and 

A  A 

Yc 

are  considered  impulsive.  At  the  point  of  collision  the  impulse  f  is  applied  as 
well.  Newton’s  second  law  and  the  law  of  moment  of  momentum  for  this  body 
are  the  following 

mvc  =  Fc(t)  +  Xc(t)  (4.1) 

JA  +  u  x  Jo;  =  p  x  Fc(t)  +  Yc(t)  (4.2) 

Taking  the  integral  over  At  and  then  the  limit  as  At  — >  0  we  get 


mAucc  =  Fc  +  Xc 

(4.3) 

JAu>c  =  p  x  Fc  +  Yc. 

(4.4) 

Notice  that  the  term  u ;  x  Jw  remains  finite  so  that  in  the  limit  its  integral  is 
zero.  The  quantity  Au£  is  the  finite  increment  of  the  absolute  linear  velocity 
of  the  body  center  mass  and  Awc  is  similarly  the  instantaneous  increment  of 
the  angular  velocity  of  the  body.  The  point  of  collision  P  experiences  a  velocity 
increment  equal  to 

Aup  =  AtT  +  Au>c  x  p.  (4-5) 

All  this  takes  place  immediately  after  the  contact.  One  should  bear  in  mind 
that  during  this  short  time  At  no  change  in  body  displacements  is  present.  The 
dynamic  equations  (4.3)  and  (4.4)  are  linear  equations  with  constant  coefficients 
for  the  unknown  velocity  increments,  impulses  and  impulse  couples. 
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In  a  general  setting,  Wittenburg  [26]  showed  that  the  instantaneous  veloc¬ 
ity  increments  and  impulses  are  related  through  linear  equations  with  constant 
coefficients  in  the  case  of  ideal  plastic  collision  and  in  the  case  of  frictionless 
collision  as  well.  These  coefficients  cannot  be  scalars  since  an  equation  of  the 
form  Av  =  aF,  where  a  is  a  scalar,  would  imply  that  Av  has  the  direction  of  F 
and  this  is  not  generally  true.  Therefore  a  relation  of  the  following  type  has  to 
be  considered 

Av  =  UF  (4.6) 

where  U  is  a  matrix  with  constant  entries  which  depend  only  on  the  state  of  the 
system  just  before  impact. 

Therefore,  by  using  (4.3)  and  (4.4)  along  with  some  kind  of  kinematic  equa¬ 
tions  of  the  form  of  (4.5),  we  can  evaluate  the  scalar  magnitude  of  impulsive 
forces  and  torques,  once  the  linear  relationship  (4.6)  between  instantaneous  ve¬ 
locity  increments  and  impulses  is  found.  This  relationship  is  provided  by  the 
differential  equations  of  motion  while  the  additional  kinematic  equation  comes 
from  our  kinematic  constraint  (3.9)  for  the  contact  point.  The  procedure  is 
summarized  below. 

Procedure:  We  assume  that  we  know  explicitly  the  positions  and  the  ve¬ 
locities  for  all  bodies  involved  in  the  process  just  before  impact.  We  take  the 
integral  of  the  equations  of  motion  from  time  to  just  prior  to  impact,  up  to  time 
(t0  +  At)  and  we  let  At  — >  0.  Keeping  in  mind  that  all  quantities  that  are 
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functions  of  generalized  coordinates  and  of  time  remain  the  same  before  and 
after  impact  and  consequently  they  are  treated  as  constants,  we  obtain  the  de¬ 
sired  relationship  between  the  increment  of  generalized  velocities  (and  angular 
ones)  and  the  external  impulses.  Some  care  should  be  taken  in  handling  terms 
depending  on  generalized  velocities  as  we  will  see  in  the  next  section. 


4.2  Impact  dynamics  of  the  flexible  beam  exper¬ 
iment 

Now,  let  us  apply  the  above  stated  procedures  in  our  case.  The  desired  relation¬ 
ships  for  angular  velocity  increments  and  the  reaction  impulse  can  be  developed 
by  the  differential  equations  of  continuous  motion  (2.15),  (2.16),  and  (2.17) 
where  /  is  considered  to  be  impulsive.  Following  the  above  procedure  we  get 


rto  +  At  rto+At  "  rto  +  At 

/  —v(t)dt  —  /  /,^]  dt  —  /  k3 

J  to  hta  J  to  J  to 


{02  -  6x)dt 


rto+At 

/  M 

Jto 


+ 


0\Jrkstsgn(dl)  +  -^-6x\dt  (4.7) 

rtn 


rto+At  rto+At 

~  /  k2(83  -  82)dt  +  /  kj {02  —  8\)dt 

Jto  Jto 

rto  +  At 

+  /  lm2el  +  ”*3/2  +  h  +  m3^3COS{03  ~  #2)]M* 
Jto 

rto+At 

+  /  [m3^l  +  h  +  rn3le3cos(83  -  82)]83dt 

Jto 


-f 

Jto 


*0  + ^ 


m3lt3sin(83  —  82){8l  —  8V)dt 
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rto+At 

4  /  //[l  4  cos(03  -  02)]<ft  (4-8) 

Jto 

/■to+At  rto+At  rio+At 

0  =  /  fc2(03  ~  G2)dt  4  /  fldt  4  /  m3l't3cos(63  —  62)02dt 

J to  ”  ^0  *^to 

rto  +  At  ^  rto  +  At 

4  /  [m3e2  4  /3]03(f<  4  /  m3le3sin(93  -  82)922dt  (4.9) 

4/to  «/to 

from  these  we  get 

0  =  4  At)  -  0jM  4  (*/r  4  +  A t)  -  e^to))  (4.10) 

0  =  [m2t\  4  m3l2  4  /2  4  m3lt3cos(03  -  92)]A82 

4[m3e^  4  h  4  m3le3cos(03  -  92)}A03  4  //[l  4  eos(03  -  02)j  (4.11) 

0  =  m3le3cos(03  —  02)A92  4  [m^  +  h]A93  4  fl  (4-12) 

Equation  (4.10)  easily  provides  that  the  instantaneous  velocity  increment  of  the 
first  body  of  the  approximation  is  zero.  This  body  experiences  instantaneous 
velocity  increment  (see  experimental  as  well  as  numerical  results),  but  this  oc¬ 
curs  only  after  some  finite  period  of  time.  In  order  to  solve  for  the  instantaneous 
velocity  increments  A 02,  A 93  and /  one  more  equation  is  needed.  1  his  equation 
as  was  mentioned  previously  comes  from  the  kinematics.  The  kinematic  con¬ 
straint  obviously  holds  just  before  and  after  the  impact.  But  since  it  depends 
only  on  angular  displacements  which  remain  the  same  for  At  — *  0,  it  does  not 
provide  any  additional  information  (i.e.  it  holds  for  both  t0  and  (i0  4  At)). 

Equation  (3.12)  was  used  to  provide  the  required  kinematics  equation.  If  0io 
and  9t0  are  the  angular  displacements  and  the  angular  velocities  of  the  bodies 
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just  before  impact,  then  we  have  for  the  time  instant  immediately  after  impact 

0  =  lcos{0  3Q  —  $20X^20  +  A02) 

+  (acos03O  +  bsin03O  -  lcos(03O  -  02o))03o  +  A03)  (4-13) 

From  the  above  we  can  easily  get 

lcos(03O  —  02O)A02  +  (acos0 3Q  -+■  bsin03O  —  lcos(03O  —  02O))A03  —  (1-14) 


where  uj0  is  defined  by 

u0  =  -lcos(03O  -  02O)02O  -  ( acos03O  +  bsinO 30  -  lcos(03O  -  02 O))03O  (4-15) 


Equations  (4.11),  (4.12)  and  (4.14)  comprise  a  system  of  linear  equations,  which 
can  be  easily  solved  for  the  instantaneous  velocity  increments  and  for  the  impul¬ 
sive  reaction  force.  In  particular,  from  (4.11)  and  (4.12)  we  have  (after  a  simple 
matrix  inversion), 


- 

- 

A02 

m3e2  + 13 

~{m3t2  +  I3  +  m3le3cos(0 30  -  02O)) 

1 

CO 

<1 

_ 1 

-m3le3cos{0 30  -  6*20 ) 

m2t\  -(-  m3l 2  +  m3le3cos(0 30  -  02O ) 

1  -f  cos(03O  —  02O) 
1 


where  A  =  (m2e2  +  m3l2  -f  I2){m3t 2  +  I3)  -  m2l2e2cos2(03O  -  02O) 


- 1 

t> 

K5 

_  ~fl 

(m3e3  +  I 3 )  COS  (  6?3q  —  02O)  —  m3lt3cos{0 30  —  #20) 

'NO 

<1 

_ 1 

A 

m2t2  +  m3l 2  +  I2  -  m3h3cos2(03O  -  02O) 

(4.16) 
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If  we  define  v0  as  follows, 


V0  =  [acos&30  +  bsind3 o  -  lcos(03o  -  02o)][m2e2  +  m3/2  ~  m3^3co52(^30  -  fJ2o)] 
+lcos(930  -  02o)[(m3e2  +  I3)cos(930  -  02O)  -  m3le3cos(930  -  02O)] 


then  by  substituting  (4.16)  in  (4.14)  we  get 


/  = 


lvn 


(4.17) 


wrhere  Uq  and  v0  depend  only  on  the  the  state  of  the  system  just  before  contact. 
Therefore,  (4.17)  gives  the  impulse  that  will  be  developed  in  the  infinitesimal 
amount  of  time  that  the  initial  contact  lasts.  Substituting  (4.17)  back  in  (4.16), 
we  get  a  closed  form  solution  for  A 02  and  A03.  The  new  velocities  immedicitely 
after  impact  for  the  second  and  the  third  bodies  easily  provide  the  following: 


^2(^0  +  At)  —  02o  +  A02,  93{t0  T  At)  —  03O  -f  A03 


(4.18) 


Remarks:  Intuitively,  we  expect  that  A02  and  A03  should  always  be  of 
different  sign.  Let  us  check  if  this  is  satisfied  by  our  closed  form  solutions. 
Notice  that  since  d2  ~  03,  cos(03  —  02)  is  always  positive  (actually  —  (rr/2)  < 
03  —  02  <  (7t/2)).  Also  t2  —  e3  =  e  =  (1/2)  and  m2  =  m3  —  m. 

[me2  +  I3  —  mle]cos(93  —  92)  ~  (me2  —  2 me2)cos(93  —  92)  <  0 

and 

me2  +  rnl2  +  I2  —  mltcos2(93  —  92)  —  me2  4-  4me2  —  2 me2cos2(63  —  02)  >  0 
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The  above  inequalities  hold  generally,  thus  looking  at  (4.16)  the  claim  is  easily 
proven. 

The  impact  theory  of  the  previous  section  takes  care  of  the  discontinuities 
in  velocities  when  contact  is  initially  achieved.  In  Wittenburg’s  analysis  this  is 
enough  to  study  the  process.  Since  he  deals  with  perfect  rigid  bodies  with  no 
deformation  at  all,  the  impact  phenomenon  is  completed  at  time  t0  +  At  and 
by  using  numerical  integration  he  obtains  the  free  motion  of  the  system  up  to  a 
possible  new  impact  etc.  But  as  was  already  explained,  the  deformation  of  the 
beam  results  in  a  force  of  finite  duration  in  our  case. 

In  a  detailed  idealized  force  profile  one  could  see  the  plot  of  Figure  4.3.  The 
initial  delta  function  corresponds  to  the  impulse  /  that  was  calculated  in  the 
previous  paragraphs.  Its  presence  is  completely  theoretical  and  not  much  can  be 
done  in  terms  of  capturing  or  controlling  it.  However,  its  results  (instantaneous 
velocity  increments)  that  determine  the  state  of  the  system  immediately  after 
initial  contact  are  of  great  importance. 
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It  is  easy  to  understand  that  the  smooth  motion  of  the  beam  just  after  the 
initial  discontinuity  is  determined  again  by  the  differential  equations  of  motion. 
In  the  ideal  case  where  the  controller  has  enough  time  available,  we  can  solve 
numerically  the  problem  of  predicting  the  maximum  reaction  force,  using  the 
methodology  of  the  previous  chapter.  Solving  a  nonlinear  system  of  equations 
is  generally  a  time-consuming  process  for  a  real-time  implementation.  A  more 
efficient  means  must  be  used  in  the  evaluation  of  the  maximum  force  on-the-fly. 
The  time  delay  in  this  procedure  should  be  a  small  percentage  of  the  granted 
sampling  period.  Some  simplifying  assumptions  are  necessary  to  solve  the  prob¬ 
lem  of  motion  immediately  after  impact  in  a  comparatively  small  amount  of 
time. 

4.2.1  Some  new  assumptions 

Consider  once  again  the  constrained  motion  of  the  beam  that  the  numerical 
methods  yielded  in  the  previous  chapter.  We  understand  that  the  delay  T 
between  the  initial  contact  and  the  time  where  the  maximum  force  is  developed, 
is  of  the  order  of  milliseconds.  For  a  mechanical  system  this  can  be  considered  a 
comparatively  short  amount  of  time.  Therefore,  an  approximation  can  be  that 
velocities  have  a  linear  relationship  with  time  for  t  G  [t0  T  At ,  +  At  +  T].  Our 

plots  agree  with  the  above  assertion.  Whether  it  is  acceptable  will  be  judged  at 
the  end. 
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0  0 


Figure  4.4:  Linear  approximation  of  velocity  of  the  ith  body  for  t  E  [t0  +  At,t0  + 
At  +  T] 


0  0 


Figure  4.5:  Quadratic  approximation  of  displacement  of  the  ith  body  for  t  E 
[t0  +  At,  t0  +  At  +  T\ 

Another  important  point  is  that  the  maximum  force  corresponds  to  zero 
angular  velocities  for  all  bodies.  Given  the  above  assumptions,  a  velocity  profile 
of  the  ith  body  is  shown  in  Figure  4.4.  Then  (4.19)  holds  trivially  for  t  E 
[^o  4*  At,  t0  +  At  +  T] 

rto  +  At+t 

0,(t)  =  0,0+  /  Hr)dr 

Jto+At 

=  0,o  +  0,(*o  +  A  <)[(<  -  to  ~  A  t)  -  (4+9) 

The  corresponding  quadratic  plot  for  displacement  is  presented  in  Figure  4.5. 
Now,  taking  the  integral  of  the  differential  equations  of  motion  from  t0  +  At  up 
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to  f0  +  At  +  T ,  where  T  is  finite,  we  get  some  similar  expressions  as  in  (4.7), 
(4.8)  and  (4.9).  These  give 

/  -fu(t)dt  =  -fc1-[(202O+-^(to  +  AO)-(20lo  +  ^i(<o  +  AO)] 

Jio  +  At  tia  l  l  z 

k  k  T 

—  I\Q\{t0  4-  At)  -(-  [kfT  4 — (4.20) 


0  —  —  k2T(03Q  —  023)  —  k2  ^  [^3(^0  d-  At)  t)2(to  +  At)] 
+^,1T(02o  —  ^10)  —  ^1  “^[^2(^0  d-  At)  —  t?j (t0  4-  At)] 

-f[m2e2  +  m3l 2  +  /2  4-  m3lt3cos{0 30  -  #2O)][0  -  02(to  4-  At)] 
4-[m3e2  4-  /3  4  m3le3cos(03O  -  02O)][O  -  03{to  4-  At)] 


d-  — /[I  4-  COs(03o  —  02o)]fn 


(4.21) 


0  =  k2T(03O- 02O)  +  k2  —  [03(to  + At)  -  02{to  + At)]  + —lfmax 

—m3U3cos(03O  -  02o)02{to  +  At)  -  [m3el  -f  /3]03(to  d-  At).  (4.22) 


These  three  equations  have  as  unknowns  the  time  delay  T,  the  maximum  reac¬ 
tion  force  fmax  and  the  velocity  0i(to+  At).  This  is  a  nonlinear  system  of  equa¬ 
tions  that  can  be  easily  solved  on-the-fly,  using  the  Newton-Raphson  method  on 
a  sufficiently  fast  processor.  The  results  at  the  end  of  the  chapter  were  obtained 
this  way. 

If  time  limitations  do  not  allow  such  a  computation,  one  more  approximation 
can  be  employed,  namely  A 0^  =  A02-  Then 

0\{to  d-  At)  =  ttjo  4-  A02 
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Here,  we  end  up  with  a  system  of  the  quadratic  equations  with  unknowns  T  and 
fmax.  If  this  is  the  case,  the  effect  of  the  input  torque  Tin(t)  on  the  system  for 
t  €  [t0  +  Af,^]  is  neglected. 

Remark:  It  is  reasonably  assumed  that  the  term  J^°+At+T  Tin(t)dt  is  given 
by  the  following  procedure:  the  controller  having  used  some  optimal  control 
method  has  come  up  with  some  optimal  Tin(i)  that  minimizes  the  effect  of 
impact,  given  the  zero  terminal  velocity  constraints  and  some  time  restrictions. 
By  the  time  that  Tin  is  prescribed,  the  above  integral  is  determined. 

The  results  of  this  method  are  discussed  at  the  end  of  the  chapter  along  with 
the  results  of  the  energy  principle  method. 


4.3  Prediction  of  the  maximum  reaction  force  us¬ 
ing  the  energy  principle 

In  recent  years,  the  problem  of  finding  closed  form  solutions  of  the  maximum 
impact  force  has  been  based  exclusively  on  the  energy  principle.  In  most  of  the 
cases  the  problem  addressed  considers  collisions  between  two  single-body  sys¬ 
tems.  Johnson  [18]  first  used  the  energy  principle  for  the  impact  force  derivation 
and  more  recently  Amirouche  [7]  extended  the  previous  method  by  incorporat¬ 
ing  the  energy  loss  due  to  structural  damping.  It  was  found  that  the  impact 
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force  equation  correlates  well  with  the  experimental  data  but  only  for  low  stiff¬ 
ness.  Similar  results  seem  to  hold  in  our  case  as  well.  This  seems  to  justify 
the  efficiency  and  the  generality  of  our  previous  method  which  works  the  same 
over  the  whole  range  of  stiffness.  More  comparisons  will  follow  at  the  end  of  the 
section. 

This  section  presents  the  derivation  of  the  maximum  reaction  force  for  the 
beam  collision  problem  using  the  energy  principle.  The  method  will  utilize  the 
kinetic  and  the  potential  energy  accumulated  at  the  beam.  However,  we  neglect 
the  energy  loss  due  to  structural  damping  of  the  beam  or  to  air  damping  (recall 
the  dimensions  of  the  beam  may  involve  substantial  air  damping).  Additionally, 
we  do  not  take  into  account  the  work  provided  by  the  motor  torque  input  from 
t0  +  At  up  to  t0  +  At  +  T,  since  it  is  comparatively  low  for  such  small  T.  In 
order  not  to  lose  the  generality  of  the  method,  we  will  frequently  refer  to  a 
Galerkin  model  of  N  rigid  bodies  connected  by  means  of  torsional  springs  of 
known  stiffness. 

A  model  representing  the  beam  colliding  with  the  rigid  wall  was  shown  in 
Figure  4.1.  This  model  predicts  the  overall  motion  changes  from  a  global  view¬ 
point.  The  impact  collision  can  be  considered  equivalent  to  one  half  oscillation 
of  this  lumped  parameter  spring-mass  system  of  Figure  4.1.  At  the  start  of  the 
collision,  the  multi-body  structure  is  at  state  {0o,6o,6o),  having  some  kinetic 
energy  K E0  and  some  potential  energy  PE0  since  even  in  the  unconstrained 
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motion  the  beam  is  deformed.  By  neglecting  the  energy  loss  due  to  air  and 
structural  damping  as  well  as  the  work  done  by  the  motor  torque  input  in  the 
time  interval  T,  the  total  energy  just  before  collision,  will  start  being  tr<ms- 
formed  into  strain  energy  for  t  >  t0  +  At.  At  the  point  of  maximum  deformation 
(which  also  corresponds  to  zero  velocities  and  to  maximum  force),  the  system 
contains  just  pure  potential  energy,  stored  in  the  torsional  springs  of  the  system. 
If  I\  E0,  PE0  and  PEmax  denote  the  total  kinetic  energy  just  before  contact,  the 
total  potential  energy  just  before  contact  and  the  total  energy  (pure  potential) 
at  the  maximum  deformation  respectively,  then 

hE0  +  PE0  =  P  Emax.  (4.23) 


The  kinetic  energy  of  each  rigid  body  just  before  contact  is  given  by  the  following 
well-known  relations: 

KEi  o  =  \lA  o 
KE20  =  ~l2&lo+\rn2c22e220 

KE 30  =  2  73^30  +  ^3^30^30 

where  s3  is  the  position  vector  of  the  center  of  mass  of  the  third  body  and  s30 
refers  to  the  configuration  immediately  before  contact. 

s3  =  [ lcosd2  +  t3cos03,  lsin02  +  e3sin03, 0]T 


and 


s3  =  [— lsin0202  —  e3sin6363^lcosd282  +  e3cosO303, 0]r 
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therefore, 


KE30  =  ^m3[/202o  +  t2Jl0  +  2lt3620630cos{d30  -  02o)]- 

The  mechanical  system  just  before  contact  has  potential  energy  which  is  given 
by 

P E0  —  -&i(02O  ~  01O)2  +  2  ^2(^30  —  ^2o)2- 
Energy  Quasi-Principle:  In  the  general  case  where  we  have  a  model  of  n 
springs  of  known  stiffness  connected  in  series,  we  postulate  that  the  following 
holds  for  the  ith' spring 

PEj,  (1/fcj) 

pe m„ 

where  {P  Elmax  /  P  Emax)  is  the  percentage  of  the  potential  energy  that  the  ith 
element  receives  at  the  maximum  deformation  configuration.  □ 

A  partial  justification  for  this  principle  may  be  derived  from  electrical  circuit 
theoretic  analogies.  We  omit  the  details.  A  more  rigorous  justification  of  the 
statement  requires  further  study2. 

In  our  case,  the  total  energy  (X^=i  E  Et0  +  PE0)  will  be  distributed  as  strain 
energy  on  the  springs  kx  and  k2  in  a  predicted  manner  according  to  (4.24).  The 
above  are  summarized  in  the  next  two  relationships 

=  YTF(PE° +  E  A'£‘o)’  (4'25) 

*  2  »=1 

2The  above  quasi-principle  assumes  that  the  maximum  deformation  configuration  corre¬ 
sponds  to  zero  velocities  for  all  rigid  bodies  of  the  system  (stationary  point).  However,  there 
might  be  cases  where  this  is  not  generally  true. 
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+  £  Ar£'">  f'1'26) 

Equations  of  this  type  for  N-body  problems  specify  angle  differences  of  the  form 
(0i+l  ,max-0i,max)‘  For  our  purposes,  this  is  sufficient  to  determine  the  maximum 
deformation  configuration  of  the  beam  (in  terms  of  position),  since  the  energy 
principle  does  not  involve  absolute  displacements3. 

At  the  state  of  maximum  deformation  (i.e.  maximum  force)  configuration 
we  also  expect  velocities  to  become  zero.  Accelerations  are  generally  non-zero 
but  there  is  no  need  of  determining  them.  Given  the  above  observations,  the 
dynamic  equations  of  motion  under  the  steady  state  of  maximum  deformation 
become: 

~7Tv(to  +  A t  +  T)  =  IiOi(t0  +  At  +  T)  -  k}(02  max  —  0l  max)  (4.27) 

Ka 

0  =  —^(^3, max  ~~  @2, max)  +  &1  [^2^0  +  At  +  T)  ~  0i(to  +  At  -f  T)] 

+  {m2e22  +  m3l2  +  I2  +  m3U3cos(93tmax  -  ^,mai)]M^o  +  A  t  +  T) 

+  [m3e3  +  h  +  rn3le3cos{03tmax  -  02,max)]^3(*o  +  A  t  +  T) 

+  /max^[l  +  COs(d3  max  -  02,max)]  (4.28) 

^2(^3, max  ^2, max)  ^'3^3<^®'^(^3,max  ^ 2 ,max)  '^ 2^ 0  4”  At  +  7  )) 

+  [m3e^  +  I3]03(to  +  A  t  +  T)  +  fmaxl  (4.29) 

3In  the  next  paragraphs  we  should  observe  that  all  displacement  terms  present  in  the 
dynamic  equations  and  in  the  kinematic  constraint  are  of  the  form  (0i+i,mo*  —  Qi,maz)- 
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Figure  4.6:  Configuration  of  the  beam  mode]  at  maximum  deformation 
Apparently,  we  need  one  more  equation  to  solve  for  the  accelerations  and  the 
force  at  the  time  instant  (t0  -f  At  +  T).  Notice  once  again,  that  the  first  equation 
is  decoupled  from  the  rest  and  it  is  unimportant.  The  additional  equation  comes 
from  the  kinematic  constraint  (3.13): 

0  =  [acos93tmax  +  bsin93  max  -  lcos{03%rnax  -  ^2,ma^)] ^3(^0  +  A/  +  T) 

+  /cos(03ijnax  —  9  2, max)  ^2^0  +  A  t  +  T)  (4.30) 

In  the  above  equation  only  known  angle  differences  are  present  except  for  the 
term  ( acos03  max  -f  bsin93  max).  This  can  be  also  written  as  a  function  of  angle 
differences  as  such  (see  Figure  4.6): 

acos03  max  +  bsin03  rriax  =  2 lcos(02<max  -  9imp)cos{93tmax  -  0imp) 

By  the  geometry  of  the  problem, 

A  _  A 

A  A  ,  ^3, max  u2 ,max  /.  01  \ 

^3, max  =  8,mp  +  - £ -  (4‘31) 

n  n 

n  a  U3,max  U2  ,max  r,n\ 

"2, max  "imp  2  '  \i-oZj 
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Hence, 


acos03  max  -f  bsinO: 


3, max  =  2,1  COS  ( - ^ )rr(^TOax  ~  = 

6  —  ft 

3.max  2, max  \ 

~2  '  = 


2lcos2(-±rna* _ 2’mai 


=  Zf!  +  COS(e3,max  ~  (>2, max)}- 


(4.33) 


Therefore,  (4.30)  becomes 


icos(03,max  ^2,max)h{^0  +  At  +  T)  +  W3(tQ  -f  A  t  +  T)  =  0.  (4.34) 

Equations  (4.28).  (4.29)  and  (4.34)  can  now  be  used  to  evaluate  the  maximum 
reaction  force  Notice  that  the  complexity  of  the  problem  is  lower  than 

that  of  the  previous  method,  since  only  one  matrix  inversion  is  involved  here. 
However,  there  is  no  means  provided  for  determining  the  delay  T  between  con¬ 
tact  time  and  time  of  maximum  force  development.  The  results  of  this  method 
follow  in  the  next  sections. 


Some  remarks  about  structural  damping:  The  above  method  can  be 
apparently  improved  by  including  the  structural  damping  of  the  beam  or/and 
the  air  damping.  As  far  as  the  structural  damping  is  concerned,  this  is  due  to 
natural  causes  inherent  to  the  mechanical  system  (material  damping,  internal 
friction,  internal  damping  or  hysteretic  damping).  In  solids,  the  nature  of  the 
internal  forces  which  produce  dissipative  effects  is  generally  very  complex  and 
varies  considerably  between  different  types  of  materials. 

For  the  continuum  equations  of  elasticity,  some  dissipative  mechanisms  such 
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as  square  root  damping  and  rate  damping  can  be  incorporated  in  the  modeling 
(a  more  detailed  analysis  can  be  found  in  [17]  ).  For  single  degree  of  freedom 
systems  as  in  Figure  4.1  ,  the  types  of  damping  encountered  are  viscous  and 
hysteretic  (linear)  as  well  as  Coulomb  damping  (nonlinear).  Unfortunately,  the 
absolute  energy  dissipation  due  to  structural  damping  for  some  given  material 
and  dimensions,  is  not  easily  determined  empirically  with  sufficient  precision. 
This  amount  of  energy  loss  may  be  negligible.  Nevertheless,  the  structural  damp¬ 
ing  does  lower  the  maximum  reaction  force  during  collision. 


4.4  Comparison  of  the  two  methods  for  evaluat¬ 
ing  the  maximum  reaction  force 

The  results  of  the  above  methods  for  evaluating  the  maximum  reaction  force 
are  presented  and  discussed  here.  The  comparisons  use  as  reference  the  detailed 
numerical  analysis  of  Chapter  3  which  we  accept  as  valid.  Each  table  refers  to  a 
specific  stiffness  of  the  beam.  As  was  mentioned  before,  all  recent  work  on  impact 
between  single-body  systems  that  used  the  energy  principle  based  method,  failed 
to  match  well  with  the  experimental  results  for  high  spring  constants.  The  first 
method  using  impact  dynamics  seemed  to  be  unaffected  over  the  whole  range  of 
beam  stiffness  and  this  is  an  important  advantage  in  terms  of  its  generality. 

For  stiffness  of  the  beam  100  X  nominal  value ,  the  maximum  reaction  force 
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2.0V 

DEM 

max  force  by  energy  principle  in  N 

35.87 

KaCTi 

82.92 

max  force  by  impact  dynamics  in  N 

27.36 

67.76 

actual  force  by  detailed  simulation  in  N 

1 WM 

26.63 

44.28 

65.36 

time  delay  by  impact  dynamics  in  sec 

0.005 

0.005 

0.006 

time  delay  by  simulation  in  sec 

0.005 

0.005 

0.005 

Table  4.1:  Prediction  results  for  k2 

=  100  X  nominal  value 

1.0V 

2.0V 

5.0V 

10. OR  | 

max  force  by  energy  principle  in  N 

7.26 

10.79 

max  force  by  impact  dynamics  in  N 

5.82 

9.18 

16.46 

23.23 

actual  force  by  detailed  simulation  in  N 

5.63 

8.72 

14.73 

22.75 

time  delay  by  impact  dynamics  in  sec 

mmmw 

time  delay  by  simulation  in  sec 

0.016 

0.015 

0.015 

0.015  | 

Table  4.2:  Prediction  results  for  k2  =  10  x  nominal  value 


as  well  as  the  delay  time  derived  by  the  impact  dynamics  method,  were  very 
close  to  those  of  the  detailed  numerical  analysis.  Recall  that  this  was  expected, 
since  the  assumption  of  linear  approximation  for,  velocities  and  displacements 
is  more  valid  as  the  duration  of  the  impact  shrinks.  The  tendency  of  the  energy 
principle  method  to  give  higher  force  values  for  the  same  stiffness,  is  because 
the  structural  damping  was  neglected. 

In  the  intermediate  case  where  k2  =  10  x  nominal  value ,  the  energy  principle 
starts  giving  acceptable  results,  but  still  the  absence  of  damping  apparently 
affects  the  method.  As  the  stiffness  value  is  decreased  to  its  nominal  value,  the 
same  method  gives  smaller  values  for  the  impact  force  which  might  confound  the 
controller.  An  explanation  is  that  the  damping  effect  is  less  severe,  but  since 
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mm 

max  force  by  energy  principle  in  N 

2.04 

2.95 

4.72 

4.73 

max  force  by  impact  dynamics  in  N 

2.05 

6.89 

8.57 

actual  force  by  detailed  simulation  in  N 

1.84 

| 

5.22 

7.43 

time  delay  by  impact  dynamics  in  sec 

0.062 

liMH 

time  delay  by  simulation  in  sec 

0.048 

limn 

giUHl 

Table  4.3:  Prediction  results  for  k2  =  nominal  value 


the  duration  of  the  pulse  is  enlarged,  the  work  provided  by  the  motor  input 
torque  becomes  more  substantial.  For  the  nominal  k2 ,  although  the  impact 
dynamics  method  could  not  provide  an  outstanding  response  for  the  delay  time, 
it  still  provides  good  results  for  the  fmax.  It  is  perhaps  worth  noticing  that  for 
a  sampling  frequency  of  100 Hz  and  for  nominal  stiffness,  the  maximum  and 
the  minimum  delays  T  are  just  one  sampling  period  far  away.  The  controller 
already  knows  in  advance  that  it  has  at  the  most  4  sampling  periods  to  respond 
in  order  to  diminish  the  effect  of  impact,  whatever  the  maximum  force  happens 
to  be.  Therefore,  the  accuracy  of  the  time  delay  T  is  of  less  importance.  As  far 
as  complexity  is  concerned,  the  energy  principle  method  is  much  simpler  since 
it  involves  just  a  matrix  inversion,  while  the  impact  dynamics  method  requires 
the  solution  of  a  comparatively  simple  nonlinear  system  of  equations. 

Since  the  next  thing  that  one  has  in  mind  after  all  this  analysis,  is  the 
design  of  the  impact  controller ,  let  us  give  here  the  most  important  advantage 
of  the  impact  dynamics  method:  the  energy  principle  method  is  just  capable  of 
providing  an  indication  on-the-fly  of  how  aggressive  the  impact  phenomenon  will 
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be  in  the  case  of  collision  and  therefore  can  advice  the  controller  to  reduce  the 
current  velocity  if  a  threshold  was  exceeded.  But  since  the  method  does  not  take 
into  account  the  term  v(t)<It  it  is  not  of  much  help  to  the  controller  in 

determining  the  optimal  control  input  v(t).  It  is  the  impact  dynamics  method 
that  can  provide  answers  to  trial  and  error  type  questions  that  may  arise  during 
the  investigation  of  the  optimal  control. 

The  results  of  this  section  serve  as  a  justification  for  the  amount  of  effort 
spent  to  understand  the  detailed  dynamics  of  the  impact  problem.  The  energy 
methods  used  in  the  past  for  the  maximum  force  prediction,  are  not  precise 
enough  to  be  used  for  controlling  impact  of  multi-body  structures. 
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Chapter  5 


Real-time  implementation  of  an 
impact-force  control  law 

5.1  Historical  perspective  of  impact  control 

As  was  mentioned  earlier,  two  methods  are  commonly  used  to  control  the  impact 
overshoot,  depending  on  the  availability  of  the  object  position  in  advance: 

1.  Given  the  position  of  the  object  in  advance  (through  some  kind  of  sensory 
feedback),  control  the  impending  contact  velocity  just  prior  to  impact. 

2.  Approach  the  workpiece  with  some  constant  velocity,  while  monitoring  the 
sensed  force  for  contact.  As  soon  as  contact  is  detected,  the  force  controller 
is  turned  on.  This  method  assumes  that  no  information  about  the  object 
position  is  available. 
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In  case  (1)  the  contact  velocity  is  controlled,  assuming  that  the  necessary 
positioning  information  is  available.  Optimal  control  theory  provides  an  ginal- 
ysis  methodology  for  addressing  the  minimal  time  problem  subject  to  the  zero 
terminal  velocity  constraint,  in  the  case  of  rigid  manipulators.  The  results  of 
this  technique  suggest  us  to  drive  the  system  with  maximum  positive  control 
effort  until  some  time  T*,  at  which  time  the  maximum  negative  control  effort  is 
applied.  Such  control  has  been  conventionally  labeled  bang-bang  control.  The 
terminal  velocity  will  be  zero  and  no  impact  will  take  place  if  the  switching  time 
T*  is  properly  chosen.  The  switching  time  T*  depends  on  the  object  shape, 
geometry  and  location.  Object  misalignment  or  robot  positioning  inaccuracies 
might  affect  the  control  severely.  Thus  the  sensory  feedback  on  object  proximity 
is  absolutely  necessary  to  determine  T*  on-the-fly.  Matters  get  more  complicated 
in  the  case  of  flexible  manipulators  where  inertia  and  flexibility  count  for  neg¬ 
ative  initial  acceleration  at  the  end-effector.  Some  simple  experiments  on  our 
flexible  beam  for  example,  proved  that  bang-bang  control  actually  results  in 
greater  reaction  forces  and  deteriorates  the  impact  problem.  Therefore,  more 
sophisticated  control  algorithms  must  be  employed  which  should  include  the 
dynamics  of  the  bodies  involved. 

The  rest  of  the  discussion  refers  to  the  more  realistic  case  (2),  where  the 
impact  controller  obtains  no  positioning  information  in  advance.  In  most  of  the 
relevant  previous  research,  the  impact  phenomenon  was  treated  by  the  same 
controller  that  was  used  to  follow  a  commanded  force.  Only  in  recent  years, 
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methods  were  suggested  to  diminish  the  impact  transient  by  tuning  the  force 
controller  gains  for  the  best  impact  response.  To  the  best  of  the  author’s  knowl¬ 
edge,  Youcef-Toumi  and  D.  Gutz  [27]  were  the  first  to  investigate  the  problem  of 
impact  in  robotic  manipulators  in  an  experimentally  based  methodology.  The 
experiments  carried  out  by  the  above-mentioned  researchers,  were  performed  on 
a  one  degree-of- freedom  dynamically  decoupled  arm,  in  a  set-up  very  similar  to 
our  flexible  beam.  Some  immediate  conclusions  of  their  work  are  listed  below: 

1.  The  allowed  approach  speed  is  directly  proportional  to  the  desired  contact 
force  level. 

2.  With  a  softer  environment  and/or  sensor,  the  gain  of  the  controller  can  be 
larger  and  consequently  a  faster  movement  of  the  arm  is  allowed1. 

3.  Integral  explicit  force  control  acts  as  a  low-pass  filter;  thus  this  type  of 
control  filters  effectively  the  high  frequency  components  of  the  impact 
transient  and  it  is  well  suited  for  force  tracking  as  well. 

Other  authors  have  addressed  the  problem  of  controlling  manipulators  sub¬ 
ject  to  impulsive  forces.  A  discontinuous  control  algorithm  that  results  in  a 
system  that  is  stable  during  both  noncontact  and  contact  modes,  was  proposed 
by  Mills  [9].  Mills  in  his  analysis  assumes  exact  knowledge  of  the  system  pa¬ 
rameters  and  the  contact  force  and  models  the  environment  as  a  surface  of 

Actually,  deliberate  compliance  in  the  combined  workpiece-sensor  -arm  dynamics  is  so  far, 
the  only  solution  consistent  with  feist  response  and  low  contact  forces. 
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finite  stifFness  and  damping.  Using  a  two  degrees-of-freedom,  five  bar  parallel 
link  configuration  operating  in  the  horizontal  plane,  he  claimed  that  the  closed 
loop  system  response  is  ultimately  bounded  for  transitions  from  noncontact  to 
contact  motion. 

Open  loop  design  schemes  were  proposed  as  well,  since  feedback  tends  to 
obscure  the  impact  effect  (because  of  inability  of  force  transducers  to  respond 
in  high  frequency  excitations).  The  computed  torque  method  proposed  by  Bayo 
[2]  uses  a  Fast  Fourier  Transform  applied  to  the  global  equations  of  motion.  The 
method,  originally  presented  for  position  control  of  open-chain  flexible  robots, 
was  well-adapted  for  force  control.  It  may  be  considered  as  an  extension  of  the 
Newton-Euler  formulation  for  the  inverse  dynamics. 

S.Eppinger  and  W.Seering  [20]  investigated  the  dynamic  modeling  of  robot 
force  control.  Active  force  control  systems  that  have  been  implemented,  demon¬ 
strated  stability  problems.  Instabilities  can  be  caused  by  link  flexibility,  actuator 
bandwidth,  digital  sampling  rate,  backlash,  bearing  friction  etc.  They  proved 
that  instability  is  present  when  the  sensor  is  located  at  a  point  remote  from  the 
actuator  (noncollocation  effect).  The  controller  in  this  case  attempts  to  regulate 
the  contact  force  through  a  dynamical  system.  Given  that,  many  researchers 
have  been  skeptical  about  deliberate  link  flexibility  and  proposed  that  every  ele¬ 
ment  between  each  actuator-sensor  pair  should  be  designed  with  high  stiffness  in 
mind.  If  this  cannot  be  avoided  (all  robots  have  some  degree  of  flexibility),  use 
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of  flexible  structures  in  the  view  of  these  researchers,  requires  either  an  exact 
system  model  or  the  use  of  an  auxiliary  collocated  sensor. 

Finally,  we  should  include  in  the  literature  review  the  naive  method  of  pre¬ 
venting  the  impact  effect  by  limiting  the  impending  contact  velocity  under  a 
certain  threshold.  For  a  robotic  arm  with  flexibility,  however,  this  threshold 
which  obviously  corresponds  to  the  worst  contact  case,  might  be  very  restric¬ 
tive.  An  explanation  of  the  above  assertion  is  that  a  certain  tip  velocity  can 
cause  different  maximum  impact  forces  at  different  given  states  of  beam  motion. 
If  the  tip  position  follows  an  oscillatory-  type  trajectory  while  on  free  motion, 
it  really  helps  to  know  if  initial  contact  is  made  in-phase  or  not.  In  particular, 
one  should  consider  the  possibility  that  the  beam  achieves  contact  while  the 
tip  has  a  negative  acceleration.  Therefore,  the  velocity  threshold  is  a  function 
of  time,  probably  of  sinusoidal  form.  The  minimum  of  this  threshold  function 
with  respect  to  time  should  be  picked,  which  apparently  results  in  a  sluggish 
movement  of  the  flexible  arm. 

Having  discussed  the  previous  schemes  for  impact  control  and  their  weak¬ 
nesses,  we  proceed  in  the  next  sections  with  a  newdy  proposed  strategy.  The 
corresponding  analysis  will  provide  justification  to  the  simplified  model  used 
and  to  the  control  algorithm. 
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5.2  The  simplified  model 


The  equations  of  motion  of  the  beam  (2. 15), (2. 16)  and  (2-.17)  may  be  restated 
in  a  more  general  form 

M(0)0  +  C(M)  +  K(0)  =  u-f  (5.1) 

where  M  and  K  are  the  conventional  finite  element  mass  and  stiffness  matrices 
respectively;  C  includes  the  nonlinear  Coriolis,  centrifugal  and  motor  damping- 
stiction  terms.  The  vector  u  contains  one  non-zero  term  only,  in  the  first  entry, 
and  that  is  the  torque  at  the  hub.  f  contains  the  unknown  reaction  forces  at 
each  body. 

In  order  to  describe  the  constrained  motion  of  the  beam,  we  should  include 
the  kinematic  constraint  equation.  Let  us  consider  the  general  case  of  the  N- 
body  Galerkin  approximation.  By  expanding  the  trigonometric  terms  of  the 
kinematic  constraint  into  Taylor  series  (see  equation  (3.9)  for  the  2nd  order 
approximation)  and  keeping  the  first  order  terms  of  the  expansion  ,  one  can  get 

the  simplified  kinematic  constraint  of  the  N-body  problem: 

N 

(5.2) 

k=\ 

where  6,mp  is  the  impact  angle  determined  at  the  tip.  Since  the  above  relation 
is  a  natural  holonomic  constraint,  its  derivatives  can  be  included  in  our  model 
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as  well,  whenever  contact  exists: 


N 

£®*  =  °  <5-3) 

k= 1 

N 

Y  ^  = 0  <5-4) 

k= 1 

which  actually  correspond  to  equations  (3.12)  and  (3.13).  It  is  clear  that  we 
have  N  + 1  dynamic  equations  (the  additional  one  reffers  to  the  hub  body),  plus 
the  natural  constraints  (5. 2), (5. 3)  and  (5.4).  However  ,  we  have  3(N  +  1)  + 
1  unknowns,  namely  the  accelerations,  velocities  and  the  displacements  of  all 
bodies,  plus  the  normal  reaction  force  at  the  tip.  Therefore,  in  order  to  solve  the 
control  problem,  we  are  forced  to  use  a  low  order  finite  element  approximation 
for  the  beam.  The  explanation  is  that  we  are  unable  to  measure  the  additional 
states  of  an  N-order  model.  Observers  that  usually  work  for  position  control  of 
flexible  structures,  seem  to  be  worthless  here,  since  the  short  settling  time  that 
is  required  for  tracking  the  impact  effect,  may  result  in  high  observer  gains  and 
instability.  Even  in  the  case  of  a  fast  analog  observer,  same  additional  delays 
will  be  involved  in  translating  the  position  information  of  the  shaft-encoders 
into  analog  format.  For  the  time  being,  only  velocity  and  angular  displacement 
of  the  shaft  are  measurable,  while  the  FSR  is  capable  of  detecting  contact  in 
a  zero-one  mode.  Its  performance  is  limited  to  capturing  low-frequency  events. 
Other  type  of  force  transducers  should  be  used  to  close  the  feedback  loop  for 
impact  control  (e.g.  piezoelectric  type),  although  their  performance  may  depend 
again  on  the  computing  power  available  and  the  interface  delays. 
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Given  the  above  considerations,  we  proceed  with  a  2nd  order  model,  justified 
by  the  insufficient  state  measurements.  Additionally,  we  will  assume  that  the 
hub  and  the  beam  are  rigidly  coupled  (fcj  =  oo),  so  that  'the  position  encoder 
measures  the  angle  62  =  0^  of  the  first  rigid  body  of  the  beam  approximation. 
Finally,  we  assume  that  the  impact  angle  6tmp  is  given  in  advance,  just  for  the 
purpose  of  evaluating  the  angle  63  on-the-fly  through  the  kinematic  constraint 
equation  (5.2). 

Note:  The  above  assumption  does  not  mean  that  we  know  the  object  po¬ 
sition  in  advance.  We  use  this  information  only  when  contact  is  already  estab¬ 
lished.  This  is  not  the  only  way  to  proceed.  We  shall  see  that  instead  of  giving 
the  impact  angle,  we  can  alternatively  measure  the  tip  force  or  the  acceleration 
at  the  hub.  Since  none  of  these  measurements  is  available  at  this  time,  the 
knowledge  of  impact  angle  was  considered  necessary. 

The  dynamic  model  of  the  2nd  order  approximation  along  with  the  natural 
constraints  are  summarized  below: 

=  W i  -  ki(°2  ~  0i )  +  +  kstsgn(6 j)  +  (5.5) 

■tha 

k2(&3  “  02 )  -  M02  ~  0i )  =  [m2tl  +  m3/2  +  /2  +  m3k3cos(03  -  92)}02 

+lm3£3  +  I3  +  m3lt3cos(83  -  02)]63 
—m3lt3sin(63  -  62){§l  -  6\) 

+//[!  +  cos{63  -  02)]  (5.6) 
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(5.7) 


-k2(63  -  e2)  =  m3le3cos(93  -  92)92  +  [m3t23  +  I3]93 
+m3h3sin(0 3  -  62)62  +  fl 


92  +  9 3  =  2 9tmp 

(5.8) 

92  +  03  =  0 

(5.9) 

^2  +  ^3  =  0 

(5.10) 

Using  the  constraint  equations  (5.8),  (5.9)  and  (5.10),  and  making  the  approxi¬ 
mation  cos(93  —  92)  ~  1,  we  can  get  a  simplified  model  of  the  constrained  motion. 
Actually,  for  the  given  stiffness  of  the  beam  and  for  the  maximum  deformation 
which  corresponds  to  the  maximum  output  motor  torque,  the  above  cosine  can 
be  found  to  be  0.995. 


TTu(0  =  kfr02  +  kstsgn(92 )  +  -^-02  ~  k2{03  -  02)  +  4 mc202  +  2 //  (5.11) 


~k2{93  —  02)  —  me202  -f  fl  (5.12) 

The  above  equations  comprise  a  simplified  model,  where  02  and  92  are  measur¬ 
able,  while  /,  9 2  and  93  are  unknown.  In  order  to  evaluate  the  complete  state 
of  our  model,  we  can  either  measure  92  or  /  or  we  can  give  9ijnp  in  advance.  By 
eliminating  the  reaction  force  from  the  above  equations  and  knowing  the  impact 
angle  ,  we  get 

^u{t)  =  kJr02  +  k3tsgn(02)  +  ^^-92  -  6 k2(0tmp  -  92)  +  2 me292  (5.13) 
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The  above  equation  will  be  the  simplified  model  of  the  constrained  motion  of 
the  beam.  Bear  in  mind  that  this  model  holds  only  when  contact  exists.  The 
reaction  force  is  not  explicitly  present  since  it  was  eliminated  using  the  constraint 
equations.  Just  for  comparison  purposes,  we  derive  a  closed  form  expression  for 
the  tip  force  using  (5.12)  above: 

/  =  ~  *2)  -  ~J~®2  (5-14) 

This  equation  gives  a  theoretical  approximation  of  the  reaction  force,  which 
complements  the  noisy  FSR  output. 


5.3  The  proposed  impact-force  control  strategy 

The  impact-force  controller  should  complete  the  following  task: 

•  approach  the  workpiece  (rigid  wall)  by  applying  a  constant  input  voltage  to 
the  actuator. 

•  as  soon  as  contact  is  established,  apply  some  desired  force  on  the  workpiece. 

The  design  criteria  should  be  small  force  overshoot,  fast  settling  time  and 
bouncing  avoidance  (i.e.  establish  contact  once  and  for  all).  Three  clear  states 
are  identified  in  this  interaction  with  the  environment:  (i)  motion  in  free  space, 
(ii)  constrained  motion  -  impact  and  (m)  constrained  motion  -  contact.  For 
these  type  of  operations  we  propose  the  following  control  scheme: 
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1.  the  controller  monitors  contact  through  the  FSR  output,  while  in  free 
space. 

2.  when  contact  is  detected,  we  specify  the  open  loop  displacement,  veloc¬ 
ity  and  acceleration  trajectories  that  the  system  should  follow,  using  the 
simplified  dynamic  model  of  the  previous  section;  we  close  the  loop  by 
penalizing  deviations  from  the  desired  displacement  and  velocity  profiles. 

3.  once  the  transient  phenomenon  is  over  and  contact  is  established,  the  force 
regulator  is  turned  on  (PD  control),  making  the  last  minor  corrections. 

The  functions  of  each  controller  mode  as  well  as  the  switching  criteria  are 
discussed  in  detail  in  the  following  paragraphs. 

The  first  controller  mode  is  just  a  naive  version  of  position  control.  For  the 
time  being,  no  care  is  devoted  to  keep  the  impending  contact  velocity  within 
appropriate  limits.  By  appropriate  limits  we  mean  not  exceeding  some  initial 
contact  velocity  threshold  that  may  result  in  instability  or  inability-to-respond 
of  the  impact  -  force  controller.  However,  the  theory  behind  this  story  is  already 
developed.  Equations  (4.20),  (4.21)  and  (4.22)  determine  the  maximum  impact 
force  that  will  be  present  at  the  tip  if  contact  is  achieved  in  the  very  next  time 
step,  given  some  voltage  input  u(t).  If  the  force  threshold  is  exceeded  then  a 
correcting  input  voltage  should  be  applied  while  still  in  free  space. 

In  parallel,  the  controller  monitors  the  reaction  force  at  the  tip  through  the 
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FSR.  When  contact  is  detected  the  controller  switches  to  the  impact  mode.  At 
this  point  we  should  keep  in  mind  that  the  force  sensor  output  of  the  most 
common  transducers  (piezoelectric,  FSR,  strain-gauge),  'goes  off-scale  during 
the  short  impact  interaction,  although  these  sensors  may  have  exceptional  rise 
delays.  Therefore,  explicit  force  feedback  terms  cannot  exclusively  determine  the 
impact  control  actions.  Open-loop  type  control  was  employed  instead,  based  on 
the  inverse  dynamics.  The  algorithm  was  borrowed  from  the  well  known  theory 
of  position  control2  and  can  be  found  in  [14].  Suppose  that  contact  is  initially 
detected  at  time  t0.  At  this  time  the  beam  is  at  some  arbitrary  state  (6,0,8). 
We  wish  to  attain  some  new  state  (6d,  6d,8d)  at  time  tj.  One  way  to  generate  a 
smooth  curve  is  by  using  a  polynomial  function  of  t.  Since  we  have  three  initial 
and  three  terminal  constraints  for  &2,  d2  and  62,  we  require  a  polynomial  with  six 
independent  coefficients  that  may  be  chosen  to  satisfy  these  constraints.  Thus, 
we  consider  a  5th  order  polynomial  of  the  form 

82(t)  —  (Z5 -(-  T  T  cl2 T  cijf  T  Qq.  (5.15) 

Then  the  desired  velocity  and  acceleration  are  automatically  given  as 


W) 

—  5astA  -f  4a4f3  +  3a3f2  -f  2a2t  +  a1, 

(5.16) 

m 

=  20a5f3  -f-  12a4t2  -f  6a3f  -f  2a2. 

(5.17) 

Without  loss  of  generality,  we  can  set  t0  =  0.  Then  from  the  initial  conditions 


2Recalll  that  force  control  can  be  considered  delicate  position  control,  especially  in  the  case 
of  flexible  manipulators. 


78 


we  can  easily  get  the  following 


a0  —  $20)  ai  —  $20)  2a2  —  $20 

From  the  terminal  conditions  we  similarly  get 

$2/  =  ast5f  +  a4t4  +  a3t3  +  a2t2  +  a^f  +  a0  (5.18) 

$2/  =  5a5t*  +  4a4t3  +  3a3t2  +  2a2t  j  +  a,}  (5.19) 

$2/  =  20a5t3 \2a4t2  +  6a3t j  +  2a2  (5.20) 

Given  the  assumptions  of  the  previous  subsection,  the  initial  state  ($20)  $20)  $20) 
is  completely  defined,  since  $2o  and  $20  are  measured  on-line,  while  the  unknown 
$20  can  be  easily  evaluated  from  equation  (5.13)  as  soon  as  contact  is  detected 

TT-u0  =  kfr02O  +  kstsgn{9 20)  +  ~^02o  -  6 k2(0imp  -  02 „)  +  2 me2620  (5.21) 

■it'd  fla 

About  the  terminal  conditions  of  the  beam,  consider  the  steady-state  mode.  In 
this  case  obviously  all  velocities  and  accelerations  become  zero. 

|u;  =  -6 =  (5.22) 

-2 M$.mP  -  $2/)  =  fjl  =»  $2/  =  $imp  +  ^  (5-23) 

where  fj  is  the  desired  reaction  force  at  the  tip  (this  is  the  actual  input  to  the 
controller).  Now,  the  unknown  coefficients  can  be  found  by  solving  the  3rd  order 
linear  system  of  equations  (5.18),  (5.19)  and  (5.20).  However,  this  pure  open 
loop  control  scheme  is  always  subject  to  external  disturbances  (recall  that  the 
dynamic  model  is  greatly  simplified).  One  improvement  might  be  to  close  the 
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loop  by  penalizing  deviations  from  the  desired  displacement  and  velocity  profiles 
since  such  dependable  feedback  exists.  The  closed  loop  gains  are  chosen  by  trial 
and  error  (some  indications  were  found  in  [23])  and  they  are  identical  to  the  ones 
used  in  the  force  controller.  The  impact  control  law  is  summarized  in  (5.24), 

~jTu  =  ikfr  +  +  kstsgn(62d)  -  6 k2(eimp  -  02d)  +  2 mt2e2d 

na  Ka 

+  Wrf  -  e2)  +  MM  -  M-  (5.24) 

After  the  application  of  the  open  loop  impact  control  for  if  seconds,  the  state 
of  the  system  is  close  or  it  should  be  close  to  the  desired  one.  It  is  an  easy  job 
for  a  simple  PD-type  control  law  to  make  the  last  corrections.  The  closed  loop 
gains  were  empirically  chosen,  while  the  constant  terminal  input  voltage  u}  was 
evaluated  using  (5.14)  since  no  dependable  force  feedback  exists3: 

u(0  ~  kpi@2d  ^2)  T  kv{@2d  ^2)  "h  uf 

=  kp(8,mp  +  02)  +  MO  -  #2)  +  {5.25) 

where  kp  =  10.15  and  kv  =  0.75. 

It  is  worth  noting  that  no  restriction  applies  on  the  switching  time  tj  from 
the  impact  control  mode  to  the  force  control  mode.  In  the  current  design  tj 
was  chosen  by  trial  and  error  under  the  criterion  of  smooth  transition  between 
modes.  Roughly  speaking,  small  tj  is  allowed  for  small  contact  velocities,  while 
bigger  one  should  be  picked  in  the  case  of  aggressive  impact  events,  where  the 
3Commanded  force  is  translated  into  desired  position 
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impact  controller  is  assigned  to  dissipate  higher  collision  energy  amounts.  The 
theoretical  justification  of  the  optimal  switching  time  is  still  an  open  subject. 


5.4  Real-time  implementation 

The  real-time  control  task  is  performed  by  an  IBM-AT  under  a  sampling  fre¬ 
quency  of  200Hz.  Lower  frequencies  proven  to  be  insufficient  for  controlling  such 
short  events  as  impact.  The  controller  uses  the  simplified  model  of  section  5.2 
from  the  time  of  initial  contact  and  thereafter.  However,  this  is  not  generally 
acceptable,  since  the  simplified  model  holds  only  when  contact  is  established.  The 
ideal  controller  should  accommodate  loss  of  contact  (and  consequently  handle 
more  aggressive  impact  effects)  by  switching  from  impact  control  back  to  some 
type  of  position  control  when  contact  is  lost.  For  the  stated  sampling  frequency, 
the  computing  power  of  the  AT  is  quite  limited,  making  such  tasks  infeasible  to 
achieve.  Obviously,  the  performance  of  the  implemented  controller  is  consider¬ 
ably  degraded  when  loss  of  contact  cannot  be  prevented. 

The  real-time  implementation  involves  accurate  knowledge  of  the  model  con¬ 
stants.  Given  that  no  dependable  analog  force  feedback  exists,  one  should  be 
very  cautious  about  the  correct  value  of  beam  stiffness  in  order  to  translate  tip 
forces  into  corresponding  beam  deviations  (body  angles).  Since  stiffness  k2  is  not 
constant  but  it  varies  non-linearly  with  the  applied  torque,  a  look-up  table  was 
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Figure  5.1:  Position  encoder  output  for  impact  response  and  desired  position 
profile 


Figure  5.2:  Tachometer  output  for  impact  response  and  desired  velocity  profile 


developed  in  advance.  The  input  to  this  table  is  the  steady-state  force  that  we 
desire  to  apply  (a  nice  way  to  evaluate  the  stiffness  of  the  beam  experimentally). 


e2f  =  o-  +  =»  A'  = - Id - 

s  v  2A'2  2  (6v-eimp) 


(5.26) 


We  keep  nominal  values  for  all  other  parameters,  presented  in  Chaipter  1. 
Figures  5.1  and  5.2  show  the  empirical  impact  response  received  by  the  position 
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encoder  and  the  tachometer  respectively  along  with  the  desired  position  and 
velocity  profiles  of  the  impact  mode.  The  current  data  hold  for  impact  angle 
0 rad  and  for  initial  input  voltage  1.0R.  The  beam  is  excited  while  it  is  at  rest  in 
an  arbitrary,  negative  position.  We  can  easily  observe  that  the  desired  position 
profiles  are  closely  followed  during  the  impact  control  phase  (the  difference  be¬ 
tween  the  experimental  data  and  the  desired  profiles  after  t  ~  0.5sec  is  because 
the  force  controller  is  turned  on).  This  is  not  the  case  for  the  velocity  profiles. 
Instability  problems  arising  from  the  abrupt  instantaneous  velocity  increments, 
do  not  allow  high  derivative  feedback  gains  in  the  impact  controller  loop. 

Figure  5.3  presents  the  noisy  FSR  output  for  the  tip  reaction  force,  which  is 
complemented  by  Figure  5.4  of  the  theoretical  tip  force  as  calculated  by  equation 
(5.14).  Figure  5.5  shows  the  control  signal  to  the  motor.  The  total  impact 
phenomenon  lasts  around  300msec,  however,  acceptable  force  level  is  applied 
after  100msec.  The  duration  of  the  impact  control  law’  is  commanded  to  be 
350msec. 

The  performance  of  the  controller  approaches  non-impact  performance  as  the 
desired  steady-state  force  increases.  Higher  desired  force  is  generally  helpful  (be¬ 
low  the  point  of  oscillatory  behavior),  since  in  the  case  that  the  beam  bounces, 
the  commanded  force  acts  to  restore  contact.  Figures  5.6  and  5.7  present  the 
corresponding  results  for  such  a  case.  Clearly,  the  impact  effect  lasts  around 
100msec. 
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Input  voltage  [V]  Theoretical  force  FSR  output 


Figure  5.3:  Force  transducer  output  for  impact  response 


Figure  5.4:  Calculated  tip  force  for  impact  response 
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Figure  5.6:  Position  and  velocity  profiles  for  higher  commanded  force 
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Figure  5.7:  Force  data  and  control  signal  to  motor  for  higher  commanded  force 


86 


Chapter  6 


Conclusions  -  suggestions  for 
further  research 


Our  work  investigates  the  effect  of  impact  that  takes  place  when  a  flexible  manip¬ 
ulator  interacts  with  the  environment.  To  summarize  what  has  been  presented 
here  : 

•  We  developed  a  finite  element  Galerkin  model  for  the  flexible  manipulator 
in  order  to  understand  the  impact  behavior  of  the  system.  The  model 
was  validated  through  experimental  results.  The  algebraic  kinematic  con¬ 
straints  that  determine  the  constrained  motion  of  the  beam  (together  with 
the  dynamic  equations)  were  specified.  The  conventional  numerical  in¬ 
tegration  methods  had  to  be  slightly  modified  to  accommodate  impact- 
contact  type  problems.  The  simulation  set-up  can  be  conveniently  used  to 
investigate  off-line  new  control  schemes  numerically. 
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•  A  detailed  mathematical  analysis  of  the  phenomena  that  take  place  just 
before  and  just  after  contact  (instantaneous  velocity  increments),  lecl  us 
to  come  up  with  a  strategy  for  predicting  the  maximum  impact  force  level 
in  an  amount  of  time  suitable  for  real-time  implementations.  The  corre¬ 
sponding  method  based  on  the  energy  principle  was  presented  for  the  sake 
of  comparison  and  its  weaknesses  were  revealed. 

•  Finally,  a  newly  proposed  control  strategy  for  reducing  the  effect  of  impact 
was  demonstrated.  Our  work  treated  the  impact  as  a  completely  separate 
event  by  including  the  corresponding  mode  into  the  controller  design.  Re¬ 
call  that  previous  designs  attempted  to  diminish  the  impact  force  by  tuning 
the  force  controller  gains  for  the  best  transition  response. 

The  current  design  is  still  in  a  quite  primitive  state  since  it  does  not  exploit 
deeper  analytical  methods  known  for  constraint  problems.  For  example,  the 
method  that  predicts  the  maximum  reaction  force  can  be  extremely  helpful  in 
designing  more  refined  position  control  laws  for  the  free  motion  of  the  manipu¬ 
lator.  These  position  control  laws  can  give  solutions  to  minimal  time  problems 
by  not  exceeding  certain  impending  contact  velocity  thresholds,  imposed  by  our 
method. 

By  providing  the  experimental  set-up  with  additional  state  measurements, 
one  can  also  lift  the  obvious  limitations  of  the  simplified  model  used  in  the  in¬ 
verse  problem  of  the  impact  control  scheme.  Such  additional  measurements  can 
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be:  dependable  force  sensing  (to  be  used  in  the  steady-state1)  and  beam  bending 
by  installing  piezo-films  on  the  flexible  arm.  The  latter  can  provide  an  estimate 
of  the  angle  differences  between  the  bodies  modeling  the  beam.  Improved  per¬ 
formance  can  also  be  expected  by  increasing  the  sampling  frequency  and/or  the 
available  computing  power. 

Closing  the  discussion,  we  should  state  some  important  facts  about  the  re¬ 
lation  between  impact  and  compliance.  It  is  well  known  that  all  manipulators 
have  some  degree  of  inherent  or  deliberate  compliance.  This  is  very  helpful,  since 
in  such  a  case  the  duration  of  impact  is  increased,  granting  the  controller  with 
more  time  to  respond.  However,  it  is  to  our  advantage  if  deliberate  compliance 
is  present  in  a  well-modeled  way.  Recall  that  accurate  modeling  is  absolutely 
necessary  in  closed-loop  endpoint  force  control,  because  it  involves  noncollocated 
sensors  and  actuators.  Therefore,  an  experimental  set-up  where  the  arm  con¬ 
sists  of  rigid  bodies  coupled  with  torsional  springs  of  known  stiffness  is  suggested 
for  consideration.  This  kind  of  system  surpasses  the  limitations  of  modeling  a 
distributed  parameter  system  and  permits  us  to  model  the  impact  problem  in  a 
more  direct  and  faithful  way. 

!It  is  worth  noting  that  in  our  work  we  intentionally  avoided  explicit  force  feedback  during 
the  impact  transient.  This  is  advised  for  future  designs  as  well,  since  almost  all  force  trans¬ 
ducers  suffer  from  saturation  during  initial  contact,  although  they  may  have  exceptional  rise 
times. 
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Appendix 


List  of  C  Source  Code  of  the  Numerical  problem 

/***************************************************************/ 


/♦  Program  impact. c  ♦  / 
/*  */ 
/*  This  program  predicts  numerically  the  free,  as  well  as  the  ♦/ 
/♦  constrained  motion  of  the  flexible  beam,  using  the  Newmark  ♦  / 
/*  approximation  along  with  Newton-Raphson  method  for  solving  */ 
/*  the  non-linear  system  of  dynamic  equation  that  results.  ♦  / 
/♦It  also  predicts  the  maximum  reaction  force  with  two  ♦  / 
/♦  different  methods  (by  impact  dynamics  and  by  the  energy  ♦  / 
/*  principle) .  */ 
/♦  The  time  step  of  the  Newmark  approximation  is  1  millisecond  */ 
/*  and  the  contact  angle  is  0.1  rad.  */ 


/***************** ** **************** *************** ******** *****/ 


#include 

<stdio ,h> 

#include 

<math .h> 

#include 

"NumRecipesDouble/nr .h" 

#include 

"NumRecipesDouble/nrutil .h" 

♦include 

"NumRecipesDouble/malloc .h" 

♦include 

"defs.h" 

double 

♦accel .♦accel_0,*accel_l; 

double 

♦vel_0 , *vel_l , ♦disp_0, *disp_ 

double 

dt .time , delay; 

double 

f orce.f _0 ,f _1 , f _max ; 

double 

input .voltage, load; 

double 

new_2 ,new_3 ; 

int 

nsteps , impact ; 

int 

f lag=0 , count=Q ,pred_on=0 ; 

mainfargc , argv) 
int  argc ; 
char  ♦♦argv; 
f 

int  s , r ; 

double  k,l; 
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double  signO; 

double  predict _by_energy() ; 

double  predict _by_dynamics() ; 

double  reeval_accel() ; 

sscanf  (argv[l]  ,  "'/,lf  "  ,&input_voltage)  ; 
if  (input_voltage<0 . 0  II  input _voltage>10 .0) 

{ 

printf("\n  invalid  argument"); 
exit (0)  ; 

} 

s  =  0 ; 

dt  =  STEP ; 

nsteps  -  =  NSTEPS; 

impact  =  FALSE; 

/**************Initialize  vectors*******************************/ 
accel_0  =  dvector(l ,3) ; 
accel_l  =  dvectorCl ,3) ; 
accel  =  dvectorCl ,3) ; 
vel_0  =  dvector(l ,3) ; 
vel_l  =  dvectorCl ,3) ; 
disp_0  =  dvectorCl  ,3) ; 
disp_l  =  dvectorCl ,3) ; 

/**************Initial  conditions*******************************/ 
disp_0[l]  =  disp_0[2]  =  disp_0[3]  =  0.0; 
vel_0  [l]  =  vel_0  [2]  =  vel_0[3]  =  0.0; 
accel_0[l]=  accel_0[2]=  accel_0[3]=  0.0; 
accel [1]  =  accel  [2]  =  accel [3]  =  0.0; 

load  =  T*input_voltage ; 

/*********** **start  simulation**********************************/ 
while (s<=nsteps) 

{ 

time  =  s/1000.0; 
if (impact) 

{ 

if (force>le-6  I  I  count==0) 

{ 

flag  =  1; 

Newmark(ITER) ; 
eount++ ; 

> 
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else  if (f orce<=le-6)  /*  release  condition  */ 

{ 

flag  =  0; 

Newmark(ITER) ; 
impact  =  FALSE; 
f_0  =  f_l  =  0.0; 

> 

> 

else 

{ 

f_0  =  f_l  =  0.0; 
flag  =  0; 

Newmark(lTER) ; 
count  =  0; 

} 

/******j|c*****Update  accelerations  and  velocities****************/ 
f or (r=l ; r<=3; r++) 

{ 

accel_l [r]=disp_l [r] -disp_0 [r] -dt*vel_0 [r] -dt*dt 
*(0.5-BETA)*accel_0[r] ; 
accel_l[r]  /=  (dt*dt*BETA) ; 

vel_l[r]  =  vel._0  [r] +dt*  (1 .0-GAMMA)  *accel_0[r]  ; 
vel_l[r]  +=  dt*GAMMA*accel_l [r] ; 

} 

/****♦*:*  I********************************************************/ 

/*  In  the  constrained  motion  we  should  modify  accelerations  */ 

/*  and  force,  because  of  the  instability  of  Newmark's  method.  */ 

/*  Velocities  and  displacements  should  be  kept  the  same.  */ 

/*  We  proceed  with  the  4-dim  system  of  equations  as  before,  */ 

/*  for  the  same  time  step.  */ 

/***************************************************************/ 
if  (force>0.0  II  count==l) 

{ 

reeval_accel() ; 

if (f orce<0 . 0)  force  =  0.0; 

/ *** a ********** ************************************ *************/ 

/*  try  to  remove  the  above  line  to  see  that  negative  forces  */ 

/*  result  from  minor  truncation  errors  and  NOT  from  */ 

/*  instability.  */ 

/*************************************************************** / 

> 

else{ 
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f or(r=l ;r<=3;r++)  accel[r]  =  accel_l[r]; 

} 

/♦♦♦♦♦♦♦♦♦♦♦♦♦♦Check  contact  condition**************************/ 
if (  (impact  &&  count==0  &&  ( (XI*sin(disp_l [3] )-PSI* 

cos(disp_l  [3]  )-L*sin(disp_l  [3]  ■‘disp.l  [2]  ))>=()) 

{ 

impact  =  TRUE; 
predict _by_energy() ; 
predict_by_dynamics () ; 
count  =  0 ; 

} 

else 

{ 

f or (r=l ; r<=3 ; r++) 

{ 

accel_0[r]  =  accel_l[r]; 
disp_0[r]  =  disp_l[r]; 
vel_0[r]  =  vel_l[r]; 

} 

f.O  =  f _1 ; 
printf ( 

'7.2. 3f  y.2.3f  */.2.3f  */,2 . 3f  */.2.3f  */,1.5f\n" 

.time ,disp_l [2] ,disp_l [3] , vel_l [2] , vel_l [3] .force) ; 
s++ ; 

} 

>  /*  end  of  ITER  */ 

f ree_vector (accel ,1,3); 
f ree_vector(accel_0 ,1,3); 
f ree_vector (accel.l ,1,3); 
free. vector (vel_0 ,1,3); 
f ree_vector(vel_l , 1,3); 
free. vector (disp.O ,1,3); 
f ree. vector (disp.l ,1,3); 

}  /*  End  of  mainO  */ 


Newmark(iter) 
int  iter; 

{ 

int  m; 

double  tolx.tolf; 
double  **alpha,*bet,  ,*x; 
void  usrfunO  ,mnewt  () ; 
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tolx=1;olf=  1.0e-6; 
if ( !pred_on) 

{ 

if ('flag)  /*  get  free  motion  */ 

{ 

alpha  =  dmatrix(l ,3, 1 ,3) ; 
bet  =  dvector (1 , 3) ; 
x  =  dvector (1 ,3) ; 
x[l]  =  disp_0[l]  ; 
x[2]  =  disp_0[2]; 
x|[3]  =  disp_0[3]; 
f or(m=l ;m<=iter ;m++) 

{ 

mnewt(l ,x,3,tolx,tolf) ; 
usrfun(x, alpha, bet) ; 

> 

d:isp_l  [1]  =  x[l]  ; 
d:isp_l  [2]  =  x  [2]  ; 
d:isp_l  [3]  =  x  [3]  ; 
f ree_vector(x, 1 ,3) ; 
f ree_vector (bet ,1,3); 
f ree_matrix (alpha, 1 ,3, 1 ,3) ; 

> 

else  /*  get  constrained  motion  */ 

-C 

alpha  =  dmatrix(l ,4, 1 ,4) ; 
bet  =  dvector(l ,4) ; 
x  =  dvector(l ,4) ; 
x[l]  =  disp_0[l]  ; 
x[2]  =  disp_0  [2]  ; 
x [3]  =  disp_0 [3] ; 
x[4]  =  f  _0 ; 
f or(m=l ;m<=iter ;m++) 

{ 

mnewt (1 , x ,4 ,tolx,tolf ) ; 
usrfun(x, alpha, bet) ; 

> 

disp_l[l]  =  x [1] ; 
disp_l  [2]  =  x [2] ; 
disp_l [3]  =  x [3]  ; 
f_l  =  x[4]  ; 
f  ree.vector (x ,1,4); 
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f ree_vector(bet ,1,4); 
f ree_matrix (alpha, 1 ,4,1,4) ; 


> 


> 

else  /*  i.e.  prediction  is  on  ♦  / 

{ 


alpha  =  dmatrix(l ,3, 1 ,3) ; 
bet  =  dvector(l ,3) ; 
x  =  dvector (1 ,3) ; 


x[l]  =  5.0 
x  [2]  =  5.0 

x[3]  =1.0;  /♦  initial  guess  for  input  voltage  IV 

for(m=l ;m<=(iter*3) ;m++) 

{ 


mnewt(l,x,3,tolx,tolf) ; 
usrfun(x, alpha, bet) ; 

} 


delay  =  x[l]  ; 
f_max  =  x [2] ; 
pred_on  =  FALSE; 
free_vector(x, 1,3) ; 
f ree_vector (bet ,1,3); 
free_matrix(alpha,l,3,l,3) ; 

> 


♦/ 


void  usrfun(x, alpha, bet) 

^^Jtc**^:^*****^*******************************:^:*/ 

/*  this  routine  determines  the  Jacobians  of  the  non-linear  */ 

/*  system  of  equations  that  results  from  the  Newmark  */ 

/*  approximation  */ 

/*************************************************************** / 


double 

♦♦alpha, *bet , *x 

{ 

int 

i; 

double 

*a,*v; 

double 

sign() ; 

double 

dif _1 ,dif_2; 

if ( ! pred_on) 

a  =  dvector (1 ,3) ; 
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v  =  dvector(l ,3) ; 

/******* ************predi ct or s* *************************** ******/ 
for(i=l ; i<=3; i++){ 

a [i] =x [i] -disp_0 [i] -dt*vel_0 [i] -dt*dt* ( . 5-BETA) *accel_0 [i] ; 
a [i] /= (dt*dt*BETA) ; 

v[i]  =  vel_0 [i] +dt* ( (1 . O-GAMMA) *accel_0 [i] +GAMMA*a[i] ) ; 

> 

/***************** Jacob ian** **************************** ********/ 
if ( !flag) 

{ 

alpha [1] [1]  =11/ (dt*dt*BETA)+Kl+(KT*KB/RA)*GAMMA/ (dt*BETA) ; 

alphaCl] [1]  +=  K_FRICT*GAMMA/(dt*BETA) ; 

alpha [l]  [2]  =-Kl; 

alpha [1] [3]  =0.0; 

alpha [2]  [1]  =-Kl; 

alpha [2]  [2]  =K1+K2+(M2*E2*E2+M3*L*L+I2+M3*L*E3 
*cos (x [3] -x  [2] ) )  / (dt*dt*BETA) ; 
alpha [2] [2]  +=a[2] *M3*L*E3*sin(x [3] -x[2] ) ; 
alpha [2]  [2]  +=a[3] *M3*L*E3*sin(x[3] -x[2] ) ; 
alpha [2]  [2]  +=M3*L*E3*cos (x [3] -x [2] ) * (v [3] *v [3] -v [2] *v [2] ) ; 
alpha[2] [2]  +=M3*L*E3*sin(x [3] -x [2] ) *2 . 0*v [2] *GAMMA 

/ (dt*BETA) ; 

alpha [2]  [3]  =-K2-a[2] *M3*L*E3*sin(x [3] -x[2] ) ; 
alpha [2]  [3]  += (M3*E3*E3+I3+M3*L*E3*cos (x [3] -x [2] ) ) 

/ (dt*dt*BETA) ; 

alpha [2]  [3]  -=a[3] *M3*L*E3*sin(x [3] -x[2] ) ; 

alpha  [2]  [3]  -=M3*L*E3*cos  (x  [3]  -x  [2]  )*(v  [3]  *v  [3]  -v  [2]  *v  [2.] )  ; 

alpha [2]  [3]  -=M3*L*E3*sin(x[3] -x [2] )*2 . 0*v [3] *GAMMA 

/ (dt*BETA) ; 

alpha [3]  [1]  =0.0; 

alpha[3] [2]  =-K2+M3*L*E3*cos (x [3] -x[2] )/ (dt*dt*BETA) ; 

alpha [3]  [2]  +=M3*L*E3*sin(x [3] -x [2] ) *a[2] ; 

alpha [3]  [2]  +=M3*iL*E3*sin(x  [3] -x [2]  ) *2 . 0*v [2]  +GAMMA 

/ (dt*BETA) ; 

alpha[3] [2]  -=M3*L*E3*cos (x [3] -x[2] ) *v [2] *v [2] ; 
alpha [3] [3]  =K2-M3*L*E3*sin(x [3] -x [2] )*a[2] ; 
alpha  [3]  [3]  +=(M3*E3*E3+I3)/(dt*dt*BETA) 

+M3*L*E3*cos(x[3]-x[2]  )*v[2]*v[2]  ; 
bet [1]  =Il*a[l] +Kl*x  [l] -Kl*x [2] +K_FRICT*(v [l] ) 

-load+(KT*KB/RA)*v[l]+STICTION*sign(v[l] ) ; 
bet  [2]  =K2*(x[2]-x[3])+Kl*(x[2]-x[l]) ; 

bet  [2]  +=(M2*E2*E2+M3*L*L+I2+M3*L*E3*cos(x [3] -x [2] ))*a[2]  ; 
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bet [2]  += (M3*E3*E3+I3+M3*L*E3*cos (x [3] -x [2] ) ) *a [3] ; 
bet  [2]  -=M3*L*E3* (sin(x [3] -x [2] ) ) * (v [3] *v [3] -v [2] *v [2] ) ; 
bet [3]  =K2* (x [3] -x [2] ) +M3*L*E3* (cos (x [3] -x [2] ) ) *a [2] ; 
bet  [3]  +=(M3*E3*E3+I3) *a[3] 

+M3*L*E3*sin (x [3] -x [2] )*v [2] *v [2] ; 
f or(i=l ; i<=3 ; i++)  bet[i]  *=  -1.0; 

> 

else 


alpha  [1]  [1] 
alpha  [1]  [1] 
alpha [1]  [2] 
alpha [1] [3] 
alpha [1] [4] 
alpha[2]  [1] 
alpha [2] [2] 

alpha [2] [2] 
alpha [2]  [2] 
alpha [2] [2] 
alpha [2]  [2] 

alpha [2] [2] 
alpha [2] [3] 
alpha [2] [3] 


alpha [2] [3] 
alpha [2] [3] 
alpha [2] [3] 


alpha [2] [3] 
alpha [2] [4] 
alpha [3]  [1] 
alpha [3] [2] 
alpha [3] [2] 
alpha [3]  [2] 


ailpha[3]  [2] 
alpha [3] [3] 
alpha [3] [3] 

ailpha[3]  [4] 
ailpha[4]  [l] 


=11/ (dt+dt ♦BETA) +K1+ (KT*KB/RA) *GAMMA/ (dt *BETA) ; 
+=K_FRICT*GAMMA/(dt*BETA) ; 

=-Kl; 

=0.0; 

=0.0; 

=-Kl ; 

=K1+K2+(M2*E2*E2+M3*L*L+I2+M3*L*E3 
♦cos (x [3] -x[2] ) )  /(dt*dt*BETA) ; 

+=a [2] *M3*L*E3*sin (x [3] -x [2] ) ; 

+=a[3] ♦M3*L*E3+sin(x[3] -x[2] ) ; 

+=M3+L*E3*cos (x [3] -x [2] ) * (v [3] *v [3] -v [2] *v [2] ) ; 
+=M3*I>E3*sin(x [3] -x [2] ) *2 . 0*v [2] ♦GAMMA 
/ (dt+BETA) ; 

+=x [4] *L*sin(x [3] -x  [2] ) ; 

=-K2-a[2] ♦M3*L*E3*sin(x[3] -x[2] ) ; 

+= (M3*E3*E3+I3+M3*L*E3*cos (x [3] -x [2] ) ) 

/ (dt*dt*BETA) ; 

-=a[3] ♦M3*L*E3*sin(x [3] -x[2] ) ; 

-=M3*L*E3*cos (x [3] -x [2] ) ♦ (v [3] *v [3] -v [2] ♦v [2] ) ; 
-=M3*L*E3*sin(x [3] -x [2] ) *2 . 0*v [3] *GAMMA 
/ (dt*BETA) ; 

-=x [4] *L*sin(x [3] -x  [2] ) ; 

=L* ( 1 . 0+cos (x [3] -x [2] ) ) ; 

=0.0; 

=-K2+M3*I>E3*cos (x [3] -x [2] ) / (dt+dt*BETA) ; 
+=M3*L*E3*sin(x [3] -x [2] )*a[2] ; 

+=M3*L*E3*s in (x [3] -x [2] ) *2 . 0*v [2] *GAMMA  . 

/ (dt*BETA) ; 

-=M3*L*E3*cos (x [3] -x [2] ) *v [2] *v [2] ; 
=K2-M3*L*E3*sin(x [3] -x [2] ) ♦a[2]  ; 

+= (M3*E3*E3+I3) / (dt ♦dt+BETA) 

+M3*L*E3*cos (x [3] -x [2] ) *v [2] *v [2]  ; 

=L; 

=0.0; 
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alpha [4] [2]  =L*cos(x  [3] -x [2] ) ; 

alpha [4] [3]  =XI*cos(x[3] )+PSI*sin(x [3] ) -L*cos(x[3] -x  [2] ) ; 
alpha [4] [4]  =0.0; 

bet  [l]  =Il*a[l]+Kl*x[l]-Kl*x[2]+K_FRICT*(v[l]) 

-load+(KT*KB/RA)*v[l]  +STICTION*sJign(v[l]  )  ; 
bet  [2]  =K2*(x[2]  -x[3]  )+Kl*(x[2]  -x[l]  )  ; 

bet  [2]  +=  (M2*E2*E2+M3*L*L+I2+M3*L*E3*cos(x  [3]  -x  [2]  )  )  *a[2]|  ; 
bet [2]  += (M3*E3*E3+I3+M3*L*E3*cos (x [3] -x [2] ) ) *a [3] ; 
bet [2]  -=M3*L*E3* (sin(x [3] -x [2] ) ) * (v [3] *v [3]  -v [2] *v [2] ) ; 
bet  [2]  +=x[4]*L*(1.0+cos(x[3]-x[2]))  ; 
bet [3]  =K2*(x [3] -x[2] )+M3*L*E3* (cos(x[3] -x [2]  )  )*a[2]  ; 
bet [3]  +=(M3*E3*E3+I3) *a[3]+M3*L*E3*sin(x [3] -x[2] ) 
*v[2]*v[2] ; 
bet [3]  +=x [4] *L ; 

bet  [4]  =XI*sin(x[3]  )-PSI*cos(x[3]  )-L*sin(x[3]-x[2]  )  ; 
f or(i=l ; i<=4; i++)  bet[i]  *=  -1.0; 

} 

> 

else  /*  i.e.  prediction  is  on  */ 

{ 

dif._2  =  disp_0[3]-disp_0[2]  ; 
dif._l  =  disp_0  [2] -disp.O  [1]  ; 

alpha[l] [1]  =-load  -Kl*dif _1-Kl*x [1] * (new_2-x [3] ) /2 . 0+ 

C (KT*KB/RA)+K_FRICT) *x [3] /2 . O+STICTION ; 

alpha [l] [2]  =0.0; 

alpha [1] [3]  =-Il+Kl*x  [1] *x[l] /4. 0+( (KT*KB/RA) 

+K_FRICT)*x[l]/2.0; 

alpha[2]  [1]  =-K2*di:f_2-K2*x[l]*(new_3-new_2)/2.0+Kl*dif_l+ 
Kl*x [1] * (new_2-x [3] ) /2 . 0+ ( 1 . 0+cos (dif _2) ) *x [2] /4 . 0 ; 
alpha [2]  [2]  =(1 . 0+cos (dif _2) )*x [1] /4 .0 ; 
alpha [2]  [3]  =-Kl*x[l]*x[l]/4.0; 
alpha[3]  [1]  =K2*dif_2+K2*x[l]*(new_3-new_2)/2.0 

+x[2]/4.0; 

alpha [3] [2]  =x[l]/4.0; 
alpha [3] [3]  =0.0; 

bet  [1]  =-Il*x [3] -Kl*x [1] *dif _1-Kl*x [1] *x[l] * (new_2-x [3] )/4. 0 
-load*x [1] + ( (KT*KB/RA) +K_FRICT) *x [l] *x [3] /2 . O+STICTION+x [1] ; 
bet  [2]  =-K2*x [1] *dif _2-K2*x [1] *x [1] * (new_3-new_2) /4 . 0 ; 
bet  [2]  +=Kl*x [1] *dif _1+Kl*x [l] *x  [1] * (new_2-x [3] )/4 . 0 ; 
bet  [2]  +=-7 . 0*M2*E2*E2*new_2-3 . 0*M2*E2*E2*new_3+ 

(1 .0+cos(dif_2))*x[l]*x[2]/4.0; 

bet [3]  =K2*x  [l] *dif _2+K2*x [l] *x [l] * (new_3-new_2) / 4.0; 
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bet  [3]  +=-2 . 0*M2*E2*E2*cos (dif _2) *new_2- (M2*E2*E2+I3)*nev_3 
+x[l]*x[2]  /4.0; 

for(i=l ; i<=3; i++)  bet[i]  *=  -1.0; 

> 


double  sign(number) 
double  number; 

{ 

double  s; 

if (number<=l .Oe-5  number>=-l . Oe-2)  s  =  0.0; 
if (number>l . Oe-5)  s  =  1.0; 
if  (numberOl .  Oe-5)  s  =  -1.0; 
return (s) ; 

} 

double  predict_by_energy() 

/***************************************************************/ 
/*  this  routine  predicts  the  maximum  reaction  force  at  the  tip  */ 
/*  using  the  energy  principle  */ 

/****** ****************************** ********************** *****/ 

{ 

double  kinetic; 
double  cosine; 

double  angle_dif ,d2_max,d3_max; 
double  f .impact; 
double  **a; 

double  **b;  /*  ax=b  */ 

a  =  dmatrix(l ,3, 1 ,3) ; 

b  =  dmatrix(l ,3, 1 ,3) ; 

cosine  =  cos (disp_0 [3] -disp_0 [2] ) ; 

kinetic  =0 . 5*Il*vel_0 [1] *vel_0 [1] ; 

kinetic  +=0 . 5*M2*E2*E2*vel_0 [2] *vel_0 [2] ; 

kinetic  +=0 . 5*M3* (L*L*vel_0 [2] *vel_0 [2] 

+E2*E2*vel_0  [3]  *vel_0  [3]+E2*vel_0  [2]  *vel_0  [3]  *cosme)  ; 
kinetic  +=0 . 5*K2* (disp_0 [3] -disp_0 [2] ) 

* (disp_0  [3] -disp_0 [2] ) ; 

/*  the  potential  portion  is  also  included  above  */ 
angle_dif  =sqrt (2 . 0*kinetic/K2) ; 
d2_max  =0 . l+angle_dif /2 . 0; 
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d3_max  =0 . l-angle_dif /2 . 0; 


aCl] [1]  =  M2*E2*E2+M3*L*L+I2+M3*L*E3*cos (angle_dif ) ; 
a[l] [2]  =  M3*E3*E3+I3+M3*L*E3*cos ( angle. dif) ; 
a[l] [3]  =  L*(l . 0+cos(angle.dif)) ; 
a[2] [1]  =  M3*L*E3*cos(angle_dif ) ; 
a[2][2]  =  M3*E3*E3+I3; 
a [2]  [3]  =  L; 

a[3] [l]  =  L*cos(angle_dif ) ; 
a [3]  [2]  =  L; 
a [3] [3]  =  0.0; 

b[l]  [1]  =  -K2*angle_dif +load; 
b[2][l]  =  K2*angle_dif ; 
b [3]  [1]  =  0.0; 

gaussj (a,3,b,3) ; 


f_impact  =  b[3][l]; 


printfC'for  K2=*/,f  and  input='/,l .  If  \n"  ,K2  ,  input  .voltage)  ; 
printf  ("prediction  by  energy:  */,1.4f  \n"  ,f .impact) ; 
f ree_matrix(a, 1 ,3,1,3); 
f ree_matrix(b , 1 ,3 , 1 ,3) ; 


double  predict.by.dynamicsO 

/***************************************************************/ 
/*  this  routine  predicts  the  maximum  reaction  force  at  the  tip  */ 
I*  using  the  impact  dynamics  */ 

/***********************************************************=M<:**/ 

{ 

double  cosine; 
double  a,b,c,d,e,f ,u; 
double  del2,del3; 

cosine  =  cos(disp_0[3]-disp_0[2] ) ; 
u  =  -(L*cosine*vel_0[2]+(XI*cos(disp_0[3]) 

+PSI*sin(disp_0 [3] )-L*cosine) *vel_0 [3] ) ; 
a  =  7.0*M2*E2*E2; 
b  =  3 . 0*M2*E2*E2 ; 
c  =  2 . 0*M2*E2*E2*cosine ; 
d  =  M2*E2*E2+I3 ; 
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e  =  (d*(l .0+cosine)-b)*(-L)/(a*d-b*c) ; 
f  =  (-c*(l .0+cosine)+a)*(-L)/(a*d-b*c) ; 
f _imp=u/ (L*cosine*e+(XI*cos(disp_0 [3] ) 

+PSI*sin(disp_0 [3] ) -L*cosine) *f ) ; 
del2  =  f_imp*e; 
del3  =  f_imp*f; 
new_2  =  vel_0 [2] +del2 ; 
new_3  =  vel_0 [3]+del3 ; 

printf ("new  2  :  */,f  \n",new_2); 
printf("new  3  :  */,f  \n",new_3); 

pred_on  =  TRUE; 

Newmark(ITER) ; 

printf ("prediction  by  dynamics  :  '/.1.4f  \n",f_max); 
printf  ("delay  :  '/.1.4f  \n"  .delay); 


double  :reeval_accel() 

{ 

double  cosine; 
double  **a; 

double  **b;  /*  ax=b  */ 

a  =  dmatrix(l ,3, 1 ,3) ; 
b  =  dmatrix(l ,3, 1 ,3) ; 

/************the  4  unknowns  are  force, accel [l] ******************/ 
cosine  =  cos(disp_l [3] -disp_l [2] ) ; 

accel[l]=load+Kl*(disp_l[2]-disp_l[l])  -K_FRICT*vel_l [1] ; 
accel [l]+=-(KT*KB/RA)*vel_l [1] -STICTION*sign(vel_l [1] ) ; 
accel [l] /=I1 ; 

a[l] [l]=M2*E2*E2+M3*L*L+I2+M3*L*E3*cosine; 
a [1] [2] =M3*E3*E3+I3+M3*L*E3*cosine ; 
a[l] [3]=L*(1 .0+cosine) ; 
b [ 1] [l]=K2*(disp_l  [3]-disp_l  [2] ) 

-Kl*(disp_l  [2]-disp_l  [l] ) ; 
b [1] [1] +=M3*L*E3*sin(disp_l [3] -disp_l  [2] ) 

*(vel_l [3] *vel_l  [3]  -vel_l [2] *vel_l  [2] ) ; 
a[2] [l]=M3*L*E3*cosine; 
a [2] [2] =M3*E3*E3+I3 ; 
a  [2]  [3]  =L; 

b [2] [l] =-K2* (disp_l [3] -disp_l [2] ) ; 
b [2] [l] +=-M3*L*E3*sin(disp_l [3] -disp_l [2] ) 
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*vel_l [2] *vel_l [2] ; 
a[3] [l]=L*cosine; 

a [3] [2]=XI*cos(disp_l [3] )+PSI*sin(disp_l [3] )-L*cosine; 
a[3] [3] =2 . 0*L*sin(disp_l [3] -disp_l [2] )*vel_l [2] *vel_l [3] ; 
b [3] [1] +=-L*sin(disp_l [3] -disp_l [2] ) *vel_;i [2]  *vel_l [2]  ; 
b[3]  [l]+=-(-XI*sin(disp_l [3] )+PSI*cos(disp_l [3] ) 

+L*sin(disp_l [3] -disp_l [2] ) )*vel_l [3] *vel_l [3] ; 

gauss j (a,3,b,3) ; 

accel[2]  =  b[l][l]; 
accel [3]  =  b [2]  [l] ; 
force  =  b [3] [l] ; 
f ree_matrix(a, 1 ,3 , 1,3); 
f ree_matrix(b , 1 ,3, 1 ,3) ; 

} 
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